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

@@ -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>([](VecTsdfVoxel &vv, const int * /* position */)
             {
                 TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(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();
 

modules/3d/src/rgbd/hash_tsdf_functions.hpp

@@ -287,7 +287,7 @@ typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> 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,

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_<float> depth(Affine3f pose) override
+    {
+        Mat_<float> frame(frameSize);
+        Reprojector reproj(intr);
+
+        Range range(0, frame.rows);
+        parallel_for_(range, RenderInvoker<CubesScene>(frame, pose, reproj, depthFactor, false /*unused*/));
+
+        return frame;
+    }
+
+    Mat_<Vec3f> rgb(Affine3f pose) override
+    {
+        Mat_<Vec3f> frame(frameSize);
+        Reprojector reproj(intr);
+
+        Range range(0, frame.rows);
+        parallel_for_(range, RenderColorInvoker<CubesScene>(frame, pose, reproj, depthFactor, false /*unused*/));
+
+        return frame;
+    }
+
+    std::vector<Affine3f> getPoses() override
+    {
+        std::vector<Affine3f> poses;
+        for (int i = 0; i < nFrames; i++)
+        {
+            Affine3f pose = startPose;
+            pose = pose.translate(shift * i);
+            poses.push_back(pose);
+        }
+
+        return poses;
+    }
+};
+
+Ptr<Scene> makeRepeatableScene(Size sz, Matx33f _intr, float _depthFactor)
+{
+    return makePtr<CubesScene>(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> scene = makeRepeatableScene(frameSize, intrIntegrate, depthFactor);
+    std::vector<Affine3f> 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 <typename VT>
 static Mat_<typename VT::value_type> normalsErrorT(Mat_<VT> srcNormals, Mat_<VT> 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<PlatformTypeEnum>
+{ };
+
+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