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

@@ -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> impl;

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

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

@@ -283,11 +283,10 @@ public:
 
 int calcVolumeUnitDegree(Point3i volumeResolution);
 
-typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
 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,
+    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(

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();

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>([](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>([](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<Vec3i> 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<Point3f> 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<float>::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;
+}
+
 }

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(); }
+
 
 }
 

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);

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<Vec4f>(); pvector < normals.end<Vec4f>(); 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<double>::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 = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
+    std::vector<Affine3f> 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<double>::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<double>::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<bool>
+{ };
+
+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<VolumeType>
+{ };
+
+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<VolumeType>
+{ };
+
+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<VolumeType>
+{ };
+
+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<std::tuple<bool, bool>>
+{ };
+
+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
 }