Merge pull request #27823 from armanrasta:5.x

Add ColorHashTSDFVolume implementation #27823

# Add ColorHashTSDFVolume implementation  [[#25155](https://github.com/opencv/opencv/issues/25155)]
## Description
Added a new ColorHashTSDFVolume implementation that combines the benefits of HashTSDFVolume's efficient spatial hashing with color support. This provides memory-efficient RGB-D fusion with better performance compared to regular ColorTSDFVolume.

### Key Features
- Hash-based spatial data structure for efficient storage
- Color integration during volume updates
- Raycast with color interpolation
- Compatible with existing TSDF interfaces
- CPU implementation with parallel processing support

### Implementation Details
- Added new ColorHashTSDFVolume class with create() factory method
- ColorVoxel structure combining TSDF and RGB data
- Spatial hashing for efficient voxel lookup
- Weighted running average for color updates
- Trilinear interpolation during raycasting
- Unit tests for basic operations and edge cases

### Files Modified/Added
- modules/3d/src/rgbd/color_hash_volume.hpp - New header defining ColorHashTSDFVolume interface
- modules/3d/src/rgbd/color_hash_volume.cpp - Implementation of ColorHashTSDFVolume
- modules/3d/test/test_color_hash_volume.cpp - Unit tests

### Performance
The implementation uses spatial hashing to only store voxels near surfaces, significantly reducing memory usage compared to regular ColorTSDFVolume while maintaining similar processing speed.

### Testing
Added unit tests that verify:
- Basic integration and raycasting operations
- Empty volume handling
- Memory usage patterns

### Future Work
- GPU/OpenCL implementation
- Additional color interpolation methods
- Extended comparison tests with other volume types

modules/ptcloud/include/opencv2/ptcloud/volume.hpp

@@ -20,7 +20,7 @@ class CV_EXPORTS_W Volume
 {
 public:
     /** @brief Constructor of custom volume.
-    * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
+    * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF, ColorHashTSDF].
     * @param settings the custom settings for volume.
     */
     CV_WRAP explicit Volume(VolumeType vtype = VolumeType::TSDF,
@@ -52,7 +52,7 @@ public:
     Camera intrinsics are taken from volume settings structure.
 
     * @param depth the depth image.
-    * @param image the color image (only for ColorTSDF).
+    * @param image the color image (only for ColorTSDF and ColorHashTSDF).
       For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
       This can be done using function registerDepth() from 3d module.
     * @param pose the pose of camera in global coordinates.
@@ -78,7 +78,7 @@ public:
     * @param cameraPose the pose of camera in global coordinates.
     * @param points image to store rendered points.
     * @param normals image to store rendered normals corresponding to points.
-    * @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
+    * @param colors image to store rendered colors corresponding to points (only for ColorTSDF and ColorHashTSDF).
     */
     CV_WRAP_AS(raycastColor) void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const;
 
@@ -106,7 +106,7 @@ public:
     * @param K camera raycast intrinsics
     * @param points image to store rendered points.
     * @param normals image to store rendered normals corresponding to points.
-    * @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
+    * @param colors image to store rendered colors corresponding to points (only for ColorTSDF and ColorHashTSDF).
     */
     CV_WRAP_AS(raycastExColor) void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals, OutputArray colors) const;
 
@@ -123,7 +123,7 @@ public:
     /** @brief Extract the all data from volume.
     * @param points the storage of all points.
     * @param normals the storage of all normals, corresponding to points.
-    * @param colors the storage of all colors, corresponding to points (only for ColorTSDF).
+    * @param colors the storage of all colors, corresponding to points (only for ColorTSDF and ColorHashTSDF).
     */
     CV_WRAP void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const;
 
@@ -157,12 +157,12 @@ public:
 
     /**
      * @brief Enables or disables new volume unit allocation during integration.
-     * Makes sense for HashTSDF only.
+     * Makes sense for HashTSDF and ColorHashTSDF only.
      */
     CV_WRAP void setEnableGrowth(bool v);
     /**
      * @brief Returns if new volume units are allocated during integration or not.
-     * Makes sense for HashTSDF only.
+     * Makes sense for HashTSDF and ColorHashTSDF only.
      */
     CV_WRAP bool getEnableGrowth() const;
 

modules/ptcloud/include/opencv2/ptcloud/volume_settings.hpp

@@ -19,7 +19,8 @@ enum class VolumeType
 {
     TSDF = 0,
     HashTSDF = 1,
-    ColorTSDF = 2
+    ColorTSDF = 2,
+    ColorHashTSDF = 3
 };
 
 
@@ -98,6 +99,26 @@ public:
     */
     CV_WRAP float getTsdfTruncateDistance() const;
 
+    /** @brief Sets gradient delta factor used for normal estimation in TSDF volumes.
+    * @param val input value.
+    */
+    CV_WRAP void setGradientDeltaFactor(float val);
+
+    /** @brief Returns gradient delta factor used for normal estimation in TSDF volumes.
+    */
+    CV_WRAP float getGradientDeltaFactor() const;
+
+    /** @brief Sets threshold for number of frames after which invisible volume units are hidden/removed.
+    * For HashTSDF and ColorHashTSDF volumes, units that haven't been visible for this many frames will be removed.
+    * @param val input value.
+    */
+    CV_WRAP void setVolumeUnitHideThreshold(int val);
+
+    /** @brief Returns threshold for number of frames after which invisible volume units are hidden/removed.
+    * For HashTSDF and ColorHashTSDF volumes, units that haven't been visible for this many frames will be removed.
+    */
+    CV_WRAP int getVolumeUnitHideThreshold() const;
+
     /** @brief Sets threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
     * @param val input value.
     */
@@ -141,16 +162,16 @@ public:
 
     /** @brief Resolution of voxel space.
         Number of voxels in each dimension.
-        Applicable only for TSDF Volume.
-        HashTSDF volume only supports equal resolution in all three dimensions.
+        Applicable only for TSDF and ColorTSDF volumes.
+        HashTSDF and ColorHashTSDF volumes only support equal resolution in all three dimensions.
     * @param val input value.
     */
     CV_WRAP void setVolumeResolution(InputArray val);
 
     /** @brief Resolution of voxel space.
         Number of voxels in each dimension.
-        Applicable only for TSDF Volume.
-        HashTSDF volume only supports equal resolution in all three dimensions.
+        Applicable only for TSDF and ColorTSDF volumes.
+        HashTSDF and ColorHashTSDF volumes only support equal resolution in all three dimensions.
     * @param val output value.
     */
     CV_WRAP void getVolumeResolution(OutputArray val) const;

modules/ptcloud/src/color_hash_tsdf_functions.cpp

@@ -0,0 +1,712 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+// ColorHashTSDF is built on top of HashTSDF: it shares the spatially-hashed
+// volume-unit storage and the raycast/fetch structure, but stores RGBTsdfVoxel
+// voxels (TSDF + running-average RGB) and integrates color alongside depth.
+// It is a CPU-only implementation.
+
+#include "precomp.hpp"
+#include "color_hash_tsdf_functions.hpp"
+
+namespace cv {
+
+namespace {
+
+inline Vec3i volumeToVolumeUnitIdx(const Point3f& point, const float volumeUnitSize)
+{
+    return cv::Vec3i(
+        cvFloor(point.x / volumeUnitSize),
+        cvFloor(point.y / volumeUnitSize),
+        cvFloor(point.z / volumeUnitSize));
+}
+
+inline cv::Point3f volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx, const float volumeUnitSize)
+{
+    return cv::Point3f(
+        volumeUnitIdx[0] * volumeUnitSize,
+        volumeUnitIdx[1] * volumeUnitSize,
+        volumeUnitIdx[2] * volumeUnitSize);
+}
+
+inline cv::Point3f voxelCoordToVolume(const cv::Vec3i& voxelIdx, const float voxelSize)
+{
+    return cv::Point3f(
+        voxelIdx[0] * voxelSize,
+        voxelIdx[1] * voxelSize,
+        voxelIdx[2] * voxelSize);
+}
+
+inline cv::Vec3i volumeToVoxelCoord(const cv::Point3f& point, const float voxelSizeInv)
+{
+    return cv::Vec3i(
+        cvFloor(point.x * voxelSizeInv),
+        cvFloor(point.y * voxelSizeInv),
+        cvFloor(point.z * voxelSizeInv));
+}
+
+inline float interpolate(float tx, float ty, float tz, float vx[8])
+{
+    float v00 = vx[0] + tz * (vx[1] - vx[0]);
+    float v01 = vx[2] + tz * (vx[3] - vx[2]);
+    float v10 = vx[4] + tz * (vx[5] - vx[4]);
+    float v11 = vx[6] + tz * (vx[7] - vx[6]);
+    float v0  = v00 + ty * (v01 - v00);
+    float v1  = v10 + ty * (v11 - v10);
+    return v0 + tx * (v1 - v0);
+}
+
+// Out-of-bounds sentinel reused from HashTSDF: tsdf == floatToTsdf(1.f) (-128), weight 0.
+RGBTsdfVoxel atColorHashVolumeUnit(
+    const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it,
+    const int volumeUnitDegree, const Vec4i volStrides)
+{
+    if (it == volumeUnits.end())
+        return RGBTsdfVoxel(floatToTsdf(1.f), 0, 0, 0, 0);
+
+    Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree,
+                                          volumeUnitIdx[1] << volumeUnitDegree,
+                                          volumeUnitIdx[2] << volumeUnitDegree);
+
+    const RGBTsdfVoxel* volData = volUnitsData.ptr<RGBTsdfVoxel>(it->second.index);
+    int coordBase = volUnitLocalIdx[0] * volStrides[0] +
+                    volUnitLocalIdx[1] * volStrides[1] +
+                    volUnitLocalIdx[2] * volStrides[2];
+    return volData[coordBase];
+}
+
+inline RGBTsdfVoxel _atColorHash(Mat& volUnitsData, const cv::Vec3i& volumeIdx, int indx,
+                                 const int volumeUnitResolution, const Vec4i volStrides)
+{
+    if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) ||
+        (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) ||
+        (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0))
+    {
+        return RGBTsdfVoxel(floatToTsdf(1.f), 0, 0, 0, 0);
+    }
+    const RGBTsdfVoxel* volData = volUnitsData.ptr<RGBTsdfVoxel>(indx);
+    int coordBase =
+        volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2];
+    return volData[coordBase];
+}
+
+// Normal estimation reuses the HashTSDF gradient scheme; only the .tsdf field is read.
+Point3f getNormalColorHashVoxel(
+    const Point3f& point, const float voxelSizeInv,
+    const int volumeUnitDegree, const Vec4i volStrides,
+    const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits)
+{
+    Vec3f normal = Vec3f(0, 0, 0);
+
+    Point3f ptVox = point * voxelSizeInv;
+    Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z));
+
+    bool queried[8];
+    VolumeUnitIndexes::const_iterator iterMap[8];
+    for (int i = 0; i < 8; i++)
+    {
+        iterMap[i] = volumeUnits.end();
+        queried[i] = false;
+    }
+
+#if !USE_INTERPOLATION_IN_GETNORMAL
+    const Vec3i offsets[] = { { 1,  0,  0}, {-1,  0,  0}, { 0,  1,  0},
+                              { 0, -1,  0}, { 0,  0,  1}, { 0,  0, -1} };
+    const int nVals = 6;
+#else
+    const Vec3i offsets[] = { { 0,  0,  0}, { 0,  0,  1}, { 0,  1,  0}, { 0,  1,  1},
+                              { 1,  0,  0}, { 1,  0,  1}, { 1,  1,  0}, { 1,  1,  1},
+                              {-1,  0,  0}, {-1,  0,  1}, {-1,  1,  0}, {-1,  1,  1},
+                              { 2,  0,  0}, { 2,  0,  1}, { 2,  1,  0}, { 2,  1,  1},
+                              { 0, -1,  0}, { 0, -1,  1}, { 1, -1,  0}, { 1, -1,  1},
+                              { 0,  2,  0}, { 0,  2,  1}, { 1,  2,  0}, { 1,  2,  1},
+                              { 0,  0, -1}, { 0,  1, -1}, { 1,  0, -1}, { 1,  1, -1},
+                              { 0,  0,  2}, { 0,  1,  2}, { 1,  0,  2}, { 1,  1,  2} };
+    const int nVals = 32;
+#endif
+
+    float vals[nVals];
+    for (int i = 0; i < nVals; i++)
+    {
+        Vec3i pt = iptVox + offsets[i];
+        Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree,
+                                    pt[1] >> volumeUnitDegree,
+                                    pt[2] >> volumeUnitDegree);
+
+        int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4;
+        auto it = iterMap[dictIdx];
+        if (!queried[dictIdx])
+        {
+            it = volumeUnits.find(volumeUnitIdx);
+            iterMap[dictIdx] = it;
+            queried[dictIdx] = true;
+        }
+        vals[i] = tsdfToFloat(atColorHashVolumeUnit(volUnitsData, volumeUnits, pt, volumeUnitIdx, it,
+                                                    volumeUnitDegree, volStrides).tsdf);
+    }
+
+#if !USE_INTERPOLATION_IN_GETNORMAL
+    for (int c = 0; c < 3; c++)
+        normal[c] = vals[c * 2] - vals[c * 2 + 1];
+#else
+    const int idxxp[8] = { 8,  9, 10, 11,  0,  1,  2,  3 };
+    const int idxxn[8] = { 4,  5,  6,  7, 12, 13, 14, 15 };
+    const int idxyp[8] = { 16, 17,  0,  1, 18, 19,  4,  5 };
+    const int idxyn[8] = { 2,  3, 20, 21,  6,  7, 22, 23 };
+    const int idxzp[8] = { 24,  0, 25,  2, 26,  4, 27,  6 };
+    const int idxzn[8] = { 1, 28,  3, 29,  5, 30,  7, 31 };
+
+    float cxv[8], cyv[8], czv[8];
+    for (int i = 0; i < 8; i++)
+    {
+        cxv[i] = vals[idxxn[i]] - vals[idxxp[i]];
+        cyv[i] = vals[idxyn[i]] - vals[idxyp[i]];
+        czv[i] = vals[idxzn[i]] - vals[idxzp[i]];
+    }
+
+    float tx = ptVox.x - iptVox[0];
+    float ty = ptVox.y - iptVox[1];
+    float tz = ptVox.z - iptVox[2];
+
+    normal[0] = interpolate(tx, ty, tz, cxv);
+    normal[1] = interpolate(tx, ty, tz, cyv);
+    normal[2] = interpolate(tx, ty, tz, czv);
+#endif
+
+    float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]);
+    return nv < 0.0001f ? nan3 : normal / nv;
+}
+
+// Trilinear color interpolation at a point given in volume coordinates (meters).
+// Neighbors are looked up across volume-unit boundaries through the hash map.
+Point3f getColorHashVoxel(
+    const Point3f& point, const float voxelSizeInv,
+    const int volumeUnitDegree, const Vec4i volStrides,
+    const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits)
+{
+    Point3f ptVox = point * voxelSizeInv;
+    Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z));
+    float tx = ptVox.x - iptVox[0];
+    float ty = ptVox.y - iptVox[1];
+    float tz = ptVox.z - iptVox[2];
+
+    const Vec3i offsets[8] = {
+        {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1},
+        {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1}
+    };
+
+    float r[8], g[8], b[8];
+    bool allValid = true;
+    for (int i = 0; i < 8; i++)
+    {
+        Vec3i pt = iptVox + offsets[i];
+        Vec3i volumeUnitIdx(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree);
+        auto it = volumeUnits.find(volumeUnitIdx);
+        RGBTsdfVoxel v = atColorHashVolumeUnit(volUnitsData, volumeUnits, pt, volumeUnitIdx, it,
+                                               volumeUnitDegree, volStrides);
+        if (v.weight == 0)
+        {
+            allValid = false;
+            break;
+        }
+        r[i] = float(v.r);
+        g[i] = float(v.g);
+        b[i] = float(v.b);
+    }
+
+    if (!allValid)
+    {
+        // Fall back to the nearest voxel if any corner of the cube is empty.
+        Vec3i volumeUnitIdx(iptVox[0] >> volumeUnitDegree,
+                            iptVox[1] >> volumeUnitDegree,
+                            iptVox[2] >> volumeUnitDegree);
+        auto it = volumeUnits.find(volumeUnitIdx);
+        RGBTsdfVoxel v = atColorHashVolumeUnit(volUnitsData, volumeUnits, iptVox, volumeUnitIdx, it,
+                                               volumeUnitDegree, volStrides);
+        return Point3f(float(v.r), float(v.g), float(v.b));
+    }
+
+    Point3f res(interpolate(tx, ty, tz, r),
+                interpolate(tx, ty, tz, g),
+                interpolate(tx, ty, tz, b));
+    colorFix(res);
+    return res;
+}
+
+} // namespace
+
+void integrateColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose,
+    int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth,
+    InputArray _depth, InputArray _rgb, InputArray _pixNorms,
+    InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(_depth.type() == DEPTH_TYPE);
+    Depth depth = _depth.getMat();
+    Mat rgb = _rgb.getMat();
+    Mat& volUnitsData = _volUnitsData.getMatRef();
+    Mat pixNorms = _pixNorms.getMat();
+
+    CV_Assert(!rgb.empty());
+    CV_Assert(depth.size() == rgb.size());
+
+    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();
+
+    Vec3i resolution;
+    settings.getVolumeResolution(resolution);
+    const float volumeUnitSize = voxelSize * resolution[0];
+
+    if (enableGrowth)
+    {
+        const int depthStride = volumeUnitDegree;
+        const float invDepthFactor = 1.f / settings.getDepthFactor();
+        const float truncDist = settings.getTsdfTruncateDistance();
+
+        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)
+        {
+            std::unordered_set<cv::Vec3i, tsdf_hash> localAccessVolUnits;
+            for (int y = range.start; y < range.end; y += depthStride)
+            {
+                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;
+
+                    Point3f camPoint = reproj(Point3f((float)x, (float)y, z));
+                    Point3f volPoint = cam2vol * camPoint;
+                    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)
+                                    localAccessVolUnits.emplace(tsdf_idx);
+                            }
+                }
+            }
+
+            mutex.lock();
+            for (const auto& tsdf_idx : localAccessVolUnits)
+            {
+                if (!newIndices.count(tsdf_idx))
+                    newIndices.emplace(tsdf_idx);
+            }
+            mutex.unlock();
+        };
+        parallel_for_(allocateRange, AllocateVolumeUnitsInvoker);
+
+        for (auto idx : newIndices)
+        {
+            VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second;
+
+            Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
+            vu.pose = subvolumePose;
+            vu.index = lastVolIndex;
+            if (lastVolIndex >= int(volUnitsData.size().height))
+            {
+                volUnitsData.resize(lastVolIndex * 2);
+                CV_LOG_DEBUG(NULL, "ColorHashTSDF storage extended from " << lastVolIndex << " to " << lastVolIndex * 2 << " volume units");
+            }
+            lastVolIndex++;
+            volUnitsData.row(vu.index).forEach<VecRGBTsdfVoxel>([](VecRGBTsdfVoxel& vv, const int* /*position*/)
+            {
+                RGBTsdfVoxel& v = reinterpret_cast<RGBTsdfVoxel&>(vv);
+                v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+                v.r = v.g = v.b = 0;
+            });
+            vu.lastVisibleIndex = frameId;
+            vu.isActive = true;
+        }
+    }
+
+    std::vector<Vec3i> totalVolUnits;
+    for (const auto& keyvalue : volumeUnits)
+        totalVolUnits.push_back(keyvalue.first);
+
+    Range inFrustumRange(0, (int)volumeUnits.size());
+    parallel_for_(inFrustumRange, [&](const Range& range)
+    {
+        const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
+        const Intr::Projector proj(intrinsics.makeProjector());
+
+        for (int i = range.start; i < range.end; ++i)
+        {
+            Vec3i tsdf_idx = totalVolUnits[i];
+            VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx);
+            if (it == volumeUnits.end())
+                continue;
+
+            Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first, volumeUnitSize);
+            Point3f volUnitInCamSpace = vol2cam * volumeUnitPos;
+            if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > maxDepth)
+            {
+                it->second.isActive = false;
+                continue;
+            }
+            Point2f cameraPoint = proj(volUnitInCamSpace);
+            if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows)
+            {
+                it->second.lastVisibleIndex = frameId;
+                it->second.isActive = true;
+            }
+        }
+    });
+
+    parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range)
+    {
+        for (int i = range.start; i < range.end; i++)
+        {
+            Vec3i tsdf_idx = totalVolUnits[i];
+            VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx);
+            if (it == volumeUnits.end())
+                return;
+
+            VolumeUnit& volumeUnit = it->second;
+            if (volumeUnit.isActive)
+            {
+                integrateColorTsdfVolumeUnit(settings, volumeUnit.pose, cameraPose,
+                                             depth, rgb, pixNorms, volUnitsData.row(volumeUnit.index));
+                volumeUnit.isActive = false;
+            }
+        }
+    });
+}
+
+void raycastColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose,
+    int height, int width, InputArray intr, const int volumeUnitDegree,
+    InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    OutputArray _points, OutputArray _normals, OutputArray _colors)
+{
+    CV_TRACE_FUNCTION();
+    Size frameSize(width, height);
+    CV_Assert(frameSize.area() > 0);
+
+    Matx33f mintr(intr.getMat());
+    Mat volUnitsData = _volUnitsData.getMat();
+
+    _points.create(frameSize, POINT_TYPE);
+    _normals.create(frameSize, POINT_TYPE);
+    if (_colors.needed())
+        _colors.create(frameSize, COLOR_TYPE);
+
+    Points points1 = _points.getMat();
+    Normals normals1 = _normals.getMat();
+    Points& points(points1);
+    Normals& normals(normals1);
+
+    Colors colors1;
+    Colors* colors = nullptr;
+    if (_colors.needed())
+    {
+        colors1 = _colors.getMat();
+        colors = &colors1;
+    }
+
+    const float truncDist = settings.getTsdfTruncateDistance();
+    const float raycastStepFactor = settings.getRaycastStepFactor();
+    const float tstep = truncDist * raycastStepFactor;
+    const float maxDepth = settings.getMaxDepth();
+    const float voxelSize = settings.getVoxelSize();
+    const float voxelSizeInv = 1.f / voxelSize;
+
+    const Vec4i volDims;
+    settings.getVolumeStrides(volDims);
+    Vec3i resolution;
+    settings.getVolumeResolution(resolution);
+    const Point3i volResolution = Point3i(resolution);
+    const float volumeUnitSize = voxelSize * resolution[0];
+
+    Matx44f _pose;
+    settings.getVolumePose(_pose);
+    const Affine3f pose = Affine3f(_pose);
+    const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
+    const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
+
+    const Intr intrinsics(mintr);
+    const Intr::Reprojector reproj(intrinsics.makeReprojector());
+
+    const int nstripes = -1;
+
+    auto HashRaycastInvoker = [&](const Range& range)
+    {
+        const Point3f cam2volTrans = cam2vol.translation();
+        const Matx33f cam2volRot = cam2vol.rotation();
+        const Matx33f vol2camRot = vol2cam.rotation();
+
+        const float blockSize = volumeUnitSize;
+
+        for (int y = range.start; y < range.end; y++)
+        {
+            ptype* ptsRow = points[y];
+            ptype* nrmRow = normals[y];
+            ptype* clrRow = colors ? (*colors)[y] : nullptr;
+
+            for (int x = 0; x < points.cols; x++)
+            {
+                Point3f point = nan3, normal = nan3, color = nan3;
+
+                Point3f orig = cam2volTrans;
+                Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f))));
+
+                float tmin = 0;
+                float tmax = maxDepth;
+                float tcurr = tmin;
+                cv::Vec3i prevVolumeUnitIdx(std::numeric_limits<int>::min(),
+                                            std::numeric_limits<int>::min(),
+                                            std::numeric_limits<int>::min());
+                float tprev = tcurr;
+                float prevTsdf = truncDist;
+                float currTsdf = truncDist;
+                (void)prevVolumeUnitIdx;
+
+                while (tcurr < tmax)
+                {
+                    Point3f currRayPos = orig + tcurr * rayDirV;
+                    cv::Vec3i currVolumeUnitIdx = volumeToVolumeUnitIdx(currRayPos, volumeUnitSize);
+
+                    VolumeUnitIndexes::const_iterator it = volumeUnits.find(currVolumeUnitIdx);
+
+                    currTsdf = prevTsdf;
+                    float stepSize = 0.5f * blockSize;
+                    cv::Vec3i volUnitLocalIdx;
+
+                    if (it != volumeUnits.end())
+                    {
+                        cv::Point3f currVolUnitPos = volumeUnitIdxToVolume(currVolumeUnitIdx, volumeUnitSize);
+                        volUnitLocalIdx = volumeToVoxelCoord(currRayPos - currVolUnitPos, voxelSizeInv);
+                        RGBTsdfVoxel currVoxel = _atColorHash(volUnitsData, volUnitLocalIdx, it->second.index,
+                                                              volResolution.x, volDims);
+
+                        currTsdf = tsdfToFloat(currVoxel.tsdf);
+
+                        if (currTsdf != prevTsdf)
+                        {
+                            // from pos to zero or negative
+                            // or from neg to zero or positive
+                            bool posCurr = currTsdf > 0.f, posPrev = prevTsdf > 0.f;
+                            bool negCurr = currTsdf < 0.f, negPrev = prevTsdf < 0.f;
+                            if (posCurr != posPrev || negCurr != negPrev)
+                                break;
+                        }
+
+                        stepSize = tstep;
+                    }
+
+                    prevVolumeUnitIdx = currVolumeUnitIdx;
+                    prevTsdf = currTsdf;
+                    tprev = tcurr;
+                    tcurr += stepSize;
+                }
+
+                if (prevTsdf >= 0.f && currTsdf <= 0.f && prevTsdf > currTsdf)
+                {
+                    float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf);
+                    if (!cvIsNaN(tInterp) && !cvIsInf(tInterp))
+                    {
+                        Point3f pv = orig + tInterp * rayDirV;
+                        Point3f nv = getNormalColorHashVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims,
+                                                                 volUnitsData, volumeUnits);
+                        if (!isNaN(nv))
+                        {
+                            normal = vol2camRot * nv;
+                            point = vol2cam * pv;
+                            if (colors)
+                                color = getColorHashVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims,
+                                                          volUnitsData, volumeUnits);
+                        }
+                    }
+                }
+
+                ptsRow[x] = toPtype(point);
+                nrmRow[x] = toPtype(normal);
+                if (clrRow)
+                    clrRow[x] = toPtype(color);
+            }
+        }
+    };
+
+    parallel_for_(Range(0, points.rows), HashRaycastInvoker, nstripes);
+}
+
+void fetchNormalsFromColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const int volumeUnitDegree, InputArray _points, OutputArray _normals)
+{
+    CV_TRACE_FUNCTION();
+
+    if (!_normals.needed())
+        return;
+
+    Points points = _points.getMat();
+    CV_Assert(points.type() == POINT_TYPE);
+
+    _normals.createSameSize(_points, _points.type());
+    Normals normals = _normals.getMat();
+    Mat volUnitsData = _volUnitsData.getMat();
+
+    const float voxelSize = settings.getVoxelSize();
+    const float voxelSizeInv = 1.f / voxelSize;
+
+    const Vec4i volDims;
+    settings.getVolumeStrides(volDims);
+
+    Matx44f _pose;
+    settings.getVolumePose(_pose);
+    const Affine3f pose(_pose);
+
+    auto HashPushNormals = [&](const ptype& point, const int* position) {
+        Affine3f invPose(pose.inv());
+        Point3f p = fromPtype(point);
+        Point3f n = nan3;
+        if (!isNaN(p))
+        {
+            Point3f voxelPoint = invPose * p;
+            n = pose.rotation() * getNormalColorHashVoxel(voxelPoint, voxelSizeInv, volumeUnitDegree,
+                                                          volDims, volUnitsData, volumeUnits);
+        }
+        normals(position[0], position[1]) = toPtype(n);
+    };
+    points.forEach(HashPushNormals);
+}
+
+void fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const int volumeUnitDegree, OutputArray _points, OutputArray _normals, OutputArray _colors)
+{
+    CV_TRACE_FUNCTION();
+
+    if (!_points.needed())
+        return;
+
+    std::vector<std::vector<ptype>> pVecs, nVecs, cVecs;
+    Mat volUnitsData = _volUnitsData.getMat();
+
+    const float voxelSize = settings.getVoxelSize();
+    const float voxelSizeInv = 1.f / voxelSize;
+
+    Vec3i resolution;
+    settings.getVolumeResolution(resolution);
+    const Point3i volResolution = Point3i(resolution);
+    const int volumeUnitResolution = volResolution.x;
+    const float volumeUnitSize = voxelSize * resolution[0];
+
+    const Vec4i volDims;
+    settings.getVolumeStrides(volDims);
+
+    Matx44f mpose;
+    settings.getVolumePose(mpose);
+    const Affine3f pose(mpose);
+
+    std::vector<Vec3i> totalVolUnits;
+    for (const auto& keyvalue : volumeUnits)
+        totalVolUnits.push_back(keyvalue.first);
+    Range fetchRange(0, (int)totalVolUnits.size());
+    const int nstripes = -1;
+
+    bool needNormals(_normals.needed());
+    bool needColors(_colors.needed());
+    Mutex mutex;
+
+    //TODO: same as HashTSDF - a 0-surface should be captured instead of all non-zero voxels
+    auto HashFetchPointsNormalsColorsInvoker = [&](const Range& range)
+    {
+        std::vector<ptype> points, normals, colors;
+        for (int i = range.start; i < range.end; i++)
+        {
+            cv::Vec3i tsdf_idx = totalVolUnits[i];
+            VolumeUnitIndexes::const_iterator it = volumeUnits.find(tsdf_idx);
+            Point3f base_point = volumeUnitIdxToVolume(tsdf_idx, volumeUnitSize);
+            if (it != volumeUnits.end())
+            {
+                std::vector<ptype> localPoints, localNormals, localColors;
+                for (int x = 0; x < volumeUnitResolution; x++)
+                    for (int y = 0; y < volumeUnitResolution; y++)
+                        for (int z = 0; z < volumeUnitResolution; z++)
+                        {
+                            cv::Vec3i voxelIdx(x, y, z);
+                            RGBTsdfVoxel voxel = _atColorHash(volUnitsData, voxelIdx, it->second.index,
+                                                              volResolution.x, volDims);
+
+                            // floatToTsdf(1.0) == -128
+                            if (voxel.tsdf != -128 && voxel.weight != 0)
+                            {
+                                Point3f point = base_point + voxelCoordToVolume(voxelIdx, voxelSize);
+                                localPoints.push_back(toPtype(pose * point));
+                                if (needNormals)
+                                {
+                                    Point3f normal = getNormalColorHashVoxel(point, voxelSizeInv, volumeUnitDegree,
+                                                                            volDims, volUnitsData, volumeUnits);
+                                    localNormals.push_back(toPtype(pose.rotation() * normal));
+                                }
+                                if (needColors)
+                                {
+                                    Point3f c(float(voxel.r), float(voxel.g), float(voxel.b));
+                                    localColors.push_back(toPtype(c));
+                                }
+                            }
+                        }
+
+                AutoLock al(mutex);
+                pVecs.push_back(localPoints);
+                nVecs.push_back(localNormals);
+                cVecs.push_back(localColors);
+            }
+        }
+    };
+
+    parallel_for_(fetchRange, HashFetchPointsNormalsColorsInvoker, nstripes);
+
+    std::vector<ptype> points, normals, colors;
+    for (size_t i = 0; i < pVecs.size(); i++)
+    {
+        points.insert(points.end(), pVecs[i].begin(), pVecs[i].end());
+        normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end());
+        colors.insert(colors.end(), cVecs[i].begin(), cVecs[i].end());
+    }
+
+    _points.create((int)points.size(), 1, POINT_TYPE);
+    if (!points.empty())
+        Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat());
+
+    if (_normals.needed())
+    {
+        _normals.create((int)normals.size(), 1, POINT_TYPE);
+        if (!normals.empty())
+            Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat());
+    }
+
+    if (_colors.needed())
+    {
+        _colors.create((int)colors.size(), 1, COLOR_TYPE);
+        if (!colors.empty())
+            Mat((int)colors.size(), 1, COLOR_TYPE, &colors[0]).copyTo(_colors.getMat());
+    }
+}
+
+} // namespace cv

modules/ptcloud/src/color_hash_tsdf_functions.hpp

@@ -0,0 +1,41 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#ifndef OPENCV_3D_COLOR_HASH_TSDF_FUNCTIONS_HPP
+#define OPENCV_3D_COLOR_HASH_TSDF_FUNCTIONS_HPP
+
+#include "hash_tsdf_functions.hpp"
+#include "color_tsdf_functions.hpp"
+
+namespace cv
+{
+
+// ColorHashTSDF uses the same voxel layout as ColorTSDF (TSDF + running-average RGB),
+// but stores voxels in spatially hashed volume units like HashTSDF.
+// It is a CPU-only implementation; the Volume::Impl interface falls back to CPU
+// regardless of the OpenCL availability (same as ColorTSDF).
+
+void integrateColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose,
+    int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth,
+    InputArray _depth, InputArray _rgb, InputArray _pixNorms,
+    InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits);
+
+void raycastColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose,
+    int height, int width, InputArray intr, const int volumeUnitDegree,
+    InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    OutputArray _points, OutputArray _normals, OutputArray _colors);
+
+void fetchNormalsFromColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const int volumeUnitDegree, InputArray _points, OutputArray _normals);
+
+void fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const int volumeUnitDegree, OutputArray _points, OutputArray _normals, OutputArray _colors);
+
+} // namespace cv
+
+#endif // OPENCV_3D_COLOR_HASH_TSDF_FUNCTIONS_HPP

modules/ptcloud/src/color_tsdf_functions.cpp

@@ -577,7 +577,7 @@ inline float interpolateColor(float tx, float ty, float tz, float vx[8])
 
 inline v_float32x4 getColorVoxel(const Mat& volume,
     const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
-    const float voxelSizeInv, const v_float32x4& p)
+    const v_float32x4& p)
 {
     if (v_check_any(v_lt(p, v_float32x4(1.f, 1.f, 1.f, 0.f))) ||
         v_check_any(v_ge(p, v_float32x4((float)(volResolution.x - 2),
@@ -586,14 +586,15 @@ inline v_float32x4 getColorVoxel(const Mat& volume,
         )
         return nanv;
 
-    v_int32x4 ip = v_floor(p);
+    const v_int32x4 ip = v_floor(p);
 
     const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
     const RGBTsdfVoxel* volData = volume.ptr<RGBTsdfVoxel>();
 
-    int ix = v_get0(ip); ip = v_rotate_right<1>(ip);
-    int iy = v_get0(ip); ip = v_rotate_right<1>(ip);
-    int iz = v_get0(ip);
+    v_int32x4 ipr = ip;
+    int ix = v_get0(ipr); ipr = v_rotate_right<1>(ipr);
+    int iy = v_get0(ipr); ipr = v_rotate_right<1>(ipr);
+    int iz = v_get0(ipr);
 
     int coordBase = ix * xdim + iy * ydim + iz * zdim;
     float CV_DECL_ALIGNED(16) rgb[4];
@@ -607,10 +608,10 @@ inline v_float32x4 getColorVoxel(const Mat& volume,
         b[i] = (float)volData[neighbourCoords[i] + coordBase].b;
     }
 
-    v_float32x4 vsi(voxelSizeInv, voxelSizeInv, voxelSizeInv, voxelSizeInv);
-    v_float32x4 ptVox = v_mul(p, vsi);
-    v_int32x4 iptVox = v_floor(ptVox);
-    v_float32x4 t = v_sub(ptVox, v_cvt_f32(iptVox));
+    // p is already in voxel-index units (corners were gathered at floor(p)),
+    // so the trilinear weights are just its fractional part. Do NOT rescale by
+    // voxelSizeInv again here — that double-scaling scrambled the weights.
+    v_float32x4 t = v_sub(p, v_cvt_f32(ip));
     float tx = v_get0(t); t = v_rotate_right<1>(t);
     float ty = v_get0(t); t = v_rotate_right<1>(t);
     float tz = v_get0(t);
@@ -630,10 +631,10 @@ inline v_float32x4 getColorVoxel(const Mat& volume,
 
 inline Point3f getColorVoxel(const Mat& volume,
     const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
-    const float voxelSizeInv, const Point3f& _p)
+    const Point3f& _p)
 {
     v_float32x4 p(_p.x, _p.y, _p.z, 0.f);
-    v_float32x4 result = getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, p);
+    v_float32x4 result = getColorVoxel(volume, volDims, neighbourCoords, volResolution, p);
     float CV_DECL_ALIGNED(16) ares[4];
     v_store_aligned(ares, result);
     return Point3f(ares[0], ares[1], ares[2]);
@@ -643,7 +644,7 @@ inline Point3f getColorVoxel(const Mat& volume,
 #else
 inline Point3f getColorVoxel(const Mat& volume,
     const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
-    const float voxelSizeInv, const Point3f& p)
+    const Point3f& p)
 {
     const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
     const RGBTsdfVoxel* volData = volume.ptr<RGBTsdfVoxel>();
@@ -670,11 +671,11 @@ inline Point3f getColorVoxel(const Mat& volume,
         b[i] = (float)volData[neighbourCoords[i] + coordBase].b;
     }
 
-    Point3f ptVox = p * voxelSizeInv;
-    Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z));
-    float tx = ptVox.x - iptVox[0];
-    float ty = ptVox.y - iptVox[1];
-    float tz = ptVox.z - iptVox[2];
+    // p is already in voxel-index units (corners gathered at floor(p) above),
+    // so weights are its fractional part — do not rescale by voxelSizeInv again.
+    float tx = p.x - ix;
+    float ty = p.y - iy;
+    float tz = p.z - iz;
 
     res = Point3f(interpolateColor(tx, ty, tz, r),
         interpolateColor(tx, ty, tz, g),
@@ -868,7 +869,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &c
                         {
                             v_float32x4 pv = v_add(orig, v_mul(dir, v_setall_f32(ts)));
                             v_float32x4 nv = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, pv);
-                            v_float32x4 cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, pv);
+                            v_float32x4 cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, pv);
 
                             if (!isNaN(nv))
                             {
@@ -980,7 +981,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &c
                         {
                             Point3f pv = (orig + dir * ts);
                             Point3f nv = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, pv);
-                            Point3f cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, pv);
+                            Point3f cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, pv);
                             if (!isNaN(nv))
                             {
                                 //convert pv and nv to camera space
@@ -1114,7 +1115,7 @@ inline void coord(
                             getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, p * voxelSizeInv)));
                     if (needColors)
                         colors.push_back(toPtype(pose.rotation() *
-                            getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, p * voxelSizeInv)));
+                            getColorVoxel(volume, volDims, neighbourCoords, volResolution, p * voxelSizeInv)));
                 }
             }
         }

modules/ptcloud/src/hash_tsdf_functions.cpp

@@ -1066,6 +1066,7 @@ void raycastHashTsdfVolumeUnit(
 
                 float tprev = tcurr;
                 float prevTsdf = truncDist;
+                float currTsdf = truncDist;
                 while (tcurr < tmax)
                 {
 
@@ -1074,8 +1075,7 @@ void raycastHashTsdfVolumeUnit(
 
                     VolumeUnitIndexes::const_iterator it = volumeUnits.find(currVolumeUnitIdx);
 
-                    float currTsdf = prevTsdf;
-                    int currWeight = 0;
+                    currTsdf = prevTsdf;
                     float stepSize = 0.5f * blockSize;
                     cv::Vec3i volUnitLocalIdx;
 
@@ -1089,32 +1089,42 @@ void raycastHashTsdfVolumeUnit(
                         //! TODO: Figure out voxel interpolation
                         TsdfVoxel currVoxel = _at(volUnitsData, volUnitLocalIdx, it->second.index, volResolution.x, volDims);
                         currTsdf = tsdfToFloat(currVoxel.tsdf);
-                        currWeight = currVoxel.weight;
+
+                        if (currTsdf != prevTsdf)
+                        {
+                            // from pos to zero or negative
+                            // or from neg to zero or positive
+                            bool posCurr = currTsdf > 0.f, posPrev = prevTsdf > 0.f;
+                            bool negCurr = currTsdf < 0.f, negPrev = prevTsdf < 0.f;
+                            if (posCurr != posPrev || negCurr != negPrev)
+                                break;
+                        }
+
                         stepSize = tstep;
                     }
 
-                    //! Surface crossing
-                    if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0)
-                    {
-                        float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf);
-                        if (!cvIsNaN(tInterp) && !cvIsInf(tInterp))
-                        {
-                            Point3f pv = orig + tInterp * rayDirV;
-                            Point3f nv = getNormalVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits);
-
-                            if (!isNaN(nv))
-                            {
-                                normal = vol2camRot * nv;
-                                point = vol2cam * pv;
-                            }
-                        }
-                        break;
-                    }
                     prevVolumeUnitIdx = currVolumeUnitIdx;
                     prevTsdf = currTsdf;
                     tprev = tcurr;
                     tcurr += stepSize;
                 }
+
+                //! Surface crossing
+                if (prevTsdf >= 0.f && currTsdf <= 0.f && prevTsdf > currTsdf)
+                {
+                    float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf);
+                    if (!cvIsNaN(tInterp) && !cvIsInf(tInterp))
+                    {
+                        Point3f pv = orig + tInterp * rayDirV;
+                        Point3f nv = getNormalVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits);
+
+                        if (!isNaN(nv))
+                        {
+                            normal = vol2camRot * nv;
+                            point = vol2cam * pv;
+                        }
+                    }
+                }
                 ptsRow[x] = toPtype(point);
                 nrmRow[x] = toPtype(normal);
             }

modules/ptcloud/src/volume_impl.cpp

@@ -8,6 +8,7 @@
 #include "tsdf_functions.hpp"
 #include "hash_tsdf_functions.hpp"
 #include "color_tsdf_functions.hpp"
+#include "color_hash_tsdf_functions.hpp"
 #include "opencv2/imgproc.hpp"
 
 namespace cv
@@ -308,37 +309,37 @@ void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, Inpu
 
     const Matx44f cameraPose = _cameraPose.getMat();
 
-#ifndef HAVE_OPENCL
-    raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals);
-#else
+#ifdef HAVE_OPENCL
     if (useGPU)
         ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals);
     else
         raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals);
+#else
+    raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals);
 #endif
 }
 
 void HashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
 {
-#ifndef HAVE_OPENCL
-    fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
-#else
+#ifdef HAVE_OPENCL
     if (useGPU)
         ocl_fetchNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
     else
         fetchNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
-
+#else
+    fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
 #endif
 }
+
 void HashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
 {
-#ifndef HAVE_OPENCL
-    fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
-#else
+#ifdef HAVE_OPENCL
     if (useGPU)
         ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
     else
         fetchPointsNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
+#else
+    fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
 #endif
 }
 
@@ -353,14 +354,7 @@ void HashTsdfVolume::reset()
     lastVolIndex = 0;
     lastFrameId = 0;
     enableGrowth = true;
-#ifndef HAVE_OPENCL
-    volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
-        {
-            TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
-            v.tsdf = floatToTsdf(0.0f); v.weight = 0;
-        });
-    volumeUnits = VolumeUnitIndexes();
-#else
+#ifdef HAVE_OPENCL
     if (useGPU)
     {
         Vec3i resolution;
@@ -387,6 +381,13 @@ void HashTsdfVolume::reset()
             });
         cpu_volumeUnits = VolumeUnitIndexes();
     }
+#else
+    volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
+        {
+            TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
+            v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+        });
+    volumeUnits = VolumeUnitIndexes();
 #endif
 }
 
@@ -417,29 +418,26 @@ void HashTsdfVolume::getBoundingBox(OutputArray boundingBox, int precision) cons
         float side = res[0] * voxelSize;
 
         std::vector<Vec3i> vi;
-#ifndef HAVE_OPENCL
-        for (const auto& keyvalue : volumeUnits)
+#ifdef HAVE_OPENCL
+    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);
-        }
+    }
 #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);
-            }
-        }
+    for (const auto& keyvalue : volumeUnits)
+        vi.push_back(keyvalue.first);
 #endif
 
+
         if (vi.empty())
         {
             boundingBox.setZero();
@@ -593,4 +591,183 @@ bool ColorTsdfVolume::getEnableGrowth() const
     return false;
 }
 
+// COLOR_HASH_TSDF
+//
+// CPU-only hash-based colored TSDF. Mirrors HashTsdfVolume's storage layout
+// (VOLUMES_SIZE volume units, spatially hashed via VolumeUnitIndexes) but uses
+// RGBTsdfVoxel and integrates color alongside depth. There is no OpenCL path
+// (same as ColorTsdfVolume); the GPU flag from Volume::Impl is ignored.
+
+ColorHashTsdfVolume::ColorHashTsdfVolume(const VolumeSettings& _settings) :
+    Volume::Impl(_settings),
+    lastVolIndex(0),
+    lastFrameId(0),
+    volumeUnitDegree(0),
+    enableGrowth(true)
+{
+    Vec3i resolution;
+    settings.getVolumeResolution(resolution);
+    const Point3i volResolution = Point3i(resolution);
+    volumeUnitDegree = calcVolumeUnitDegree(volResolution);
+
+    volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<RGBTsdfVoxel>());
+    reset();
+}
+
+ColorHashTsdfVolume::~ColorHashTsdfVolume() {}
+
+void ColorHashTsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
+{
+    CV_TRACE_FUNCTION();
+    Mat depth, image;
+    frame.getDepth(depth);
+    frame.getImage(image);
+    integrate(depth, image, _cameraPose);
+}
+
+void ColorHashTsdfVolume::integrate(InputArray, InputArray)
+{
+    CV_Error(cv::Error::StsBadFunc, "Color data should be passed for this volume type");
+}
+
+void ColorHashTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray _cameraPose)
+{
+    Mat depth = _depth.getMat();
+    Mat image = _image.getMat();
+    const Matx44f cameraPose = _cameraPose.getMat();
+    Matx33f intr;
+    settings.getCameraIntegrateIntrinsics(intr);
+    Intr intrinsics(intr);
+    Vec6f newParams((float)depth.rows, (float)depth.cols,
+        intrinsics.fx, intrinsics.fy,
+        intrinsics.cx, intrinsics.cy);
+    if (!(frameParams == newParams))
+    {
+        frameParams = newParams;
+        preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
+    }
+
+    integrateColorHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId,
+                                     volumeUnitDegree, enableGrowth,
+                                     depth, image, pixNorms, volUnitsData, volumeUnits);
+    lastFrameId++;
+}
+
+void ColorHashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors)  const
+{
+    Matx33f intr;
+    settings.getCameraRaycastIntrinsics(intr);
+    raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
+}
+
+void ColorHashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
+{
+    const Matx44f cameraPose = _cameraPose.getMat();
+    raycastColorHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree,
+                                   volUnitsData, volumeUnits, _points, _normals, _colors);
+}
+
+void ColorHashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
+{
+    fetchNormalsFromColorHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree,
+                                            points, normals);
+}
+
+void ColorHashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
+{
+    fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree,
+                                                       points, normals, noArray());
+}
+
+void ColorHashTsdfVolume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const
+{
+    fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree,
+                                                       points, normals, colors);
+}
+
+void ColorHashTsdfVolume::reset()
+{
+    CV_TRACE_FUNCTION();
+    lastVolIndex = 0;
+    lastFrameId = 0;
+    enableGrowth = true;
+    volUnitsData.forEach<VecRGBTsdfVoxel>([](VecRGBTsdfVoxel& vv, const int* /* position */)
+        {
+            RGBTsdfVoxel& v = reinterpret_cast<RGBTsdfVoxel&>(vv);
+            v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+            v.r = v.g = v.b = 0;
+        });
+    volumeUnits = VolumeUnitIndexes();
+}
+
+int ColorHashTsdfVolume::getVisibleBlocks() const { return (int)volumeUnits.size(); }
+size_t ColorHashTsdfVolume::getTotalVolumeUnits() const { return volumeUnits.size(); }
+
+void ColorHashTsdfVolume::setEnableGrowth(bool v)
+{
+    enableGrowth = v;
+}
+
+bool ColorHashTsdfVolume::getEnableGrowth() const
+{
+    return enableGrowth;
+}
+
+void ColorHashTsdfVolume::getBoundingBox(OutputArray boundingBox, int precision) const
+{
+    // Same VOLUME_UNIT-level bounding box as HashTsdfVolume, derived from the
+    // set of allocated volume units.
+    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;
+        for (const auto& keyvalue : volumeUnits)
+            vi.push_back(keyvalue.first);
+
+        if (vi.empty())
+        {
+            boundingBox.setZero();
+        }
+        else
+        {
+            std::vector<Point3f> pts;
+            for (Vec3i idx : vi)
+            {
+                Point3f base = Point3f((float)idx[0], (float)idx[1], (float)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)
+            {
+                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);
+            }
+
+            bb.copyTo(boundingBox);
+        }
+    }
+}
+
 }

modules/ptcloud/src/volume_impl.hpp

@@ -211,6 +211,55 @@ private:
 };
 
 
+// ColorHashTSDF is a CPU-only hash-based colored TSDF volume.
+// It does not inherit ColorTsdfVolume (which owns a dense volume Mat) on purpose:
+// the whole point of hashing is to avoid allocating the dense volume.
+class ColorHashTsdfVolume : public Volume::Impl
+{
+public:
+    ColorHashTsdfVolume(const VolumeSettings& settings);
+    ~ColorHashTsdfVolume();
+
+    virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
+    virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
+    virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void fetchNormals(InputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void reset() override;
+    virtual int getVisibleBlocks() const override;
+    virtual size_t getTotalVolumeUnits() const override;
+
+    // Enables or disables new volume unit allocation during integration
+    // Applicable for HashTSDF and ColorHashTSDF only
+    virtual void setEnableGrowth(bool v) override;
+    // Returns if new volume units are allocated during integration or not
+    // Applicable for HashTSDF and ColorHashTSDF 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 void getBoundingBox(OutputArray bb, int precision) const override;
+
+public:
+    int lastVolIndex;
+    int lastFrameId;
+    Vec6f frameParams;
+    int volumeUnitDegree;
+    bool enableGrowth;
+
+    Mat volUnitsData;
+    Mat pixNorms;
+    VolumeUnitIndexes volumeUnits;
+};
+
+
 Volume::Volume(VolumeType vtype, const VolumeSettings& settings)
 {
     switch (vtype)
@@ -224,8 +273,11 @@ Volume::Volume(VolumeType vtype, const VolumeSettings& settings)
     case VolumeType::ColorTSDF:
         this->impl = makePtr<ColorTsdfVolume>(settings);
         break;
+    case VolumeType::ColorHashTSDF:
+        this->impl = makePtr<ColorHashTsdfVolume>(settings);
+        break;
     default:
-        CV_Error(Error::StsInternal, "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }");
+        CV_Error(Error::StsInternal, "Incorrect VolumeType");
         break;
     }
 }
@@ -254,4 +306,4 @@ bool Volume::getEnableGrowth() const { return this->impl->getEnableGrowth(); }
 
 }
 
-#endif // !OPENCV_3D_VOLUME_IMPL_HPP
+#endif // OPENCV_3D_VOLUME_IMPL_HPP

modules/ptcloud/src/volume_settings_impl.cpp

@@ -56,6 +56,10 @@ public:
     virtual int   getMaxWeight() const = 0;
     virtual void  setRaycastStepFactor(float val) = 0;
     virtual float getRaycastStepFactor() const = 0;
+    virtual void  setGradientDeltaFactor(float val) = 0;
+    virtual float getGradientDeltaFactor() const = 0;
+    virtual void  setVolumeUnitHideThreshold(int val) = 0;
+    virtual int   getVolumeUnitHideThreshold() const = 0;
 
     virtual void setVolumePose(InputArray val) = 0;
     virtual void getVolumePose(OutputArray val) const = 0;
@@ -95,6 +99,10 @@ public:
     virtual int   getMaxWeight() const override;
     virtual void  setRaycastStepFactor(float val) override;
     virtual float getRaycastStepFactor() const override;
+    virtual void  setGradientDeltaFactor(float val) override;
+    virtual float getGradientDeltaFactor() const override;
+    virtual void  setVolumeUnitHideThreshold(int val) override;
+    virtual int   getVolumeUnitHideThreshold() const override;
 
     virtual void setVolumePose(InputArray val) override;
     virtual void getVolumePose(OutputArray val) const override;
@@ -120,6 +128,8 @@ private:
     int   maxWeight;
     float raycastStepFactor;
     bool  zFirstMemOrder;
+    float gradientDeltaFactor;
+    int   volumeUnitHideThreshold;
 
     Matx44f volumePose;
     Point3i volumeResolution;
@@ -156,13 +166,15 @@ public:
         static const int maxWeight = 64; // number of frames
         static constexpr float raycastStepFactor = 0.75f;
         static const bool zFirstMemOrder = true; // order of voxels in volume
+        static constexpr float gradientDeltaFactor = 1.0f;
+        static const int volumeUnitHideThreshold = 10;
 
         const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
         const Matx44f  volumePoseMatrix = volumePose.matrix;
         // Unlike original code, this should work with any volume size
         // Not only when (x,y,z % 32) == 0
         const Point3i  volumeResolution = Vec3i::all(128); //number of voxels
-    };
+};
 
     class DefaultHashTsdfSets {
     public:
@@ -190,6 +202,8 @@ public:
         static const int maxWeight = 64; // number of frames
         static constexpr float raycastStepFactor = 0.25f;
         static const bool zFirstMemOrder = true; // order of voxels in volume
+        static constexpr float gradientDeltaFactor = 1.0f;
+        static const int volumeUnitHideThreshold = 10;
 
         const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
         const Matx44f  volumePoseMatrix = volumePose.matrix;
@@ -232,6 +246,8 @@ public:
         static const int maxWeight = 64; // number of frames
         static constexpr float raycastStepFactor = 0.75f;
         static const bool zFirstMemOrder = true; // order of voxels in volume
+        static constexpr float gradientDeltaFactor = 1.0f;
+        static const int volumeUnitHideThreshold = 10;
 
         const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
         const Matx44f  volumePoseMatrix = volumePose.matrix;
@@ -240,6 +256,42 @@ public:
         const Point3i  volumeResolution = Vec3i::all(128); //number of voxels
     };
 
+    class DefaultColorHashTsdfSets {
+    public:
+        static const int integrateWidth = 640;
+        static const int integrateHeight = 480;
+        float ifx = 525.f; // focus point x axis
+        float ify = 525.f; // focus point y axis
+        float icx = float(integrateWidth) / 2.f - 0.5f;  // central point x axis
+        float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
+
+        static const int raycastWidth = 640;
+        static const int raycastHeight = 480;
+        float rfx = 525.f; // focus point x axis
+        float rfy = 525.f; // focus point y axis
+        float rcx = float(raycastWidth) / 2.f - 0.5f;  // central point x axis
+        float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
+
+        static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
+        static constexpr float volumeSize = 3.f; // meters
+        static constexpr float voxelSize = volumeSize / 512.f; //meters (similar to HashTSDF)
+        static constexpr float tsdfTruncateDistance = 7 * voxelSize; // similar to HashTSDF
+        static constexpr float maxDepth = 4.f;
+        static const int maxWeight = 64; // number of frames
+        static constexpr float raycastStepFactor = 0.25f; // similar to HashTSDF
+        static const bool zFirstMemOrder = true; // order of voxels in volume
+        static constexpr float gradientDeltaFactor = 1.0f;
+        static const int volumeUnitHideThreshold = 10;
+
+        const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
+        const Matx44f  volumePoseMatrix = volumePose.matrix;
+        // Unlike original code, this should work with any volume size
+        // Not only when (x,y,z % 32) == 0
+        const Point3i  volumeResolution = Vec3i::all(16); //number of voxels (similar to HashTSDF)
+};
+
 };
 
 
@@ -291,7 +343,10 @@ void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl->
 void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); };
 void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); };
 void VolumeSettings::getCameraRaycastIntrinsics(OutputArray val) const { this->impl->getCameraRaycastIntrinsics(val); };
-
+void  VolumeSettings::setGradientDeltaFactor(float val) { this->impl->setGradientDeltaFactor(val); };
+float VolumeSettings::getGradientDeltaFactor() const { return this->impl->getGradientDeltaFactor(); };
+void  VolumeSettings::setVolumeUnitHideThreshold(int val) { this->impl->setVolumeUnitHideThreshold(val); };
+int   VolumeSettings::getVolumeUnitHideThreshold() const { return this->impl->getVolumeUnitHideThreshold(); };
 
 VolumeSettingsImpl::VolumeSettingsImpl()
     : VolumeSettingsImpl(VolumeType::TSDF)
@@ -322,6 +377,8 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
         this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
         this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
         this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+        this->gradientDeltaFactor = ds.gradientDeltaFactor;
+        this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold;
     }
     else if (volumeType == VolumeType::HashTSDF)
     {
@@ -344,6 +401,8 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
         this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
         this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
         this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+        this->gradientDeltaFactor = ds.gradientDeltaFactor;
+        this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold;
     }
     else if (volumeType == VolumeType::ColorTSDF)
     {
@@ -366,6 +425,32 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
         this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
         this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
         this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+        this->gradientDeltaFactor = ds.gradientDeltaFactor;
+        this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold;
+    }
+    else if (volumeType == VolumeType::ColorHashTSDF)
+    {
+        DefaultColorHashTsdfSets ds = DefaultColorHashTsdfSets();
+
+        this->integrateWidth = ds.integrateWidth;
+        this->integrateHeight = ds.integrateHeight;
+        this->raycastWidth = ds.raycastWidth;
+        this->raycastHeight = ds.raycastHeight;
+        this->depthFactor = ds.depthFactor;
+        this->voxelSize = ds.voxelSize;
+        this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
+        this->maxDepth = ds.maxDepth;
+        this->maxWeight = ds.maxWeight;
+        this->raycastStepFactor = ds.raycastStepFactor;
+        this->zFirstMemOrder = ds.zFirstMemOrder;
+
+        this->volumePose = ds.volumePoseMatrix;
+        this->volumeResolution = ds.volumeResolution;
+        this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
+        this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
+        this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+        this->gradientDeltaFactor = ds.gradientDeltaFactor;
+        this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold;
     }
 }
 
@@ -532,4 +617,26 @@ void VolumeSettingsImpl::getCameraRaycastIntrinsics(OutputArray val) const
     Mat(this->cameraRaycastIntrinsics).copyTo(val);
 }
 
+void VolumeSettingsImpl::setGradientDeltaFactor(float val)
+{
+    this->gradientDeltaFactor = val;
+}
+
+float VolumeSettingsImpl::getGradientDeltaFactor() const
+{
+    return this->gradientDeltaFactor;
+}
+
+void VolumeSettingsImpl::setVolumeUnitHideThreshold(int val)
+{
+    this->volumeUnitHideThreshold = val;
+}
+
+int VolumeSettingsImpl::getVolumeUnitHideThreshold() const
+{
+    return this->volumeUnitHideThreshold;
+}
+
+
+
 }

modules/ptcloud/test/test_tsdf.cpp

@@ -517,11 +517,11 @@ void staticBoundingBoxTest(VolumeType volumeType)
 }
 
 
-// For HashTSDF only
-void boundingBoxGrowthTest(bool enableGrowth)
+// For HashTSDF and ColorHashTSDF
+void boundingBoxGrowthTest(VolumeType volumeType, bool enableGrowth)
 {
-    VolumeSettings vs(VolumeType::HashTSDF);
-    Volume volume(VolumeType::HashTSDF, vs);
+    VolumeSettings vs(volumeType);
+    Volume volume(volumeType, vs);
 
     Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
     Matx33f intrIntegrate, intrRaycast;
@@ -536,11 +536,25 @@ void boundingBoxGrowthTest(bool enableGrowth)
     UMat udepth;
     depth.copyTo(udepth);
 
+    const bool isColor = (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF);
+    Mat rgb;
+    UMat urgb;
+    if (isColor)
+    {
+        rgb = scene->rgb(poses[0]);
+        rgb.copyTo(urgb);
+    }
+
     // 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);
+    {
+        if (isColor)
+            volume.integrate(udepth, urgb, poses[0].matrix);
+        else
+            volume.integrate(udepth, poses[0].matrix);
+    }
 
     Vec6f bb;
     volume.getBoundingBox(bb, Volume::BoundingBoxPrecision::VOLUME_UNIT);
@@ -569,7 +583,12 @@ void boundingBoxGrowthTest(bool enableGrowth)
     volume.setEnableGrowth(enableGrowth);
 
     for (int i = 0; i < nIntegrations; i++)
-        volume.integrate(udepth2, poses[0].matrix);
+    {
+        if (isColor)
+            volume.integrate(udepth2, urgb, poses[0].matrix);
+        else
+            volume.integrate(udepth2, poses[0].matrix);
+    }
 
     Vec6f bb2;
     volume.getBoundingBox(bb2, Volume::BoundingBoxPrecision::VOLUME_UNIT);
@@ -675,12 +694,12 @@ Ptr<Scene> makeRepeatableScene(Size sz, Matx33f _intr, float _depthFactor)
     return makePtr<CubesScene>(sz, _intr, _depthFactor);
 }
 
-// For HashTSDF only
-void hugeSceneGrowthTest()
+// For HashTSDF and ColorHashTSDF
+void hugeSceneGrowthTest(VolumeType volumeType)
 {
-    VolumeSettings vs(VolumeType::HashTSDF);
+    VolumeSettings vs(volumeType);
     vs.setMaxDepth(10);
-    Volume volume(VolumeType::HashTSDF, vs);
+    Volume volume(volumeType, vs);
 
     Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
     Matx33f intrIntegrate, intrRaycast;
@@ -690,12 +709,24 @@ void hugeSceneGrowthTest()
     Ptr<Scene> scene = makeRepeatableScene(frameSize, intrIntegrate, depthFactor);
     std::vector<Affine3f> poses = scene->getPoses();
 
+    const bool isColor = (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF);
+
     // 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 (isColor)
+    {
+        Mat rgb = scene->rgb(poses[0]);
+        UMat urgb;
+        rgb.copyTo(urgb);
+        volume.integrate(udepth, urgb, poses[0].matrix);
+    }
+    else
+    {
+        volume.integrate(udepth, poses[0].matrix);
+    }
     if (cvtest::debugLevel > 0)
     {
         debugVolumeDraw(volume, poses[0], depth, depthFactor, "pts.obj");
@@ -769,18 +800,18 @@ static Mat normalsError(Mat srcNormals, Mat dstNormals)
     return Mat();
 }
 
-void regressionVolPoseRot()
+void regressionVolPoseRot(VolumeType volumeType)
 {
     // Make 2 volumes which differ only in their pose (especially rotation)
-    VolumeSettings vs(VolumeType::HashTSDF);
-    Volume volume0(VolumeType::HashTSDF, vs);
+    VolumeSettings vs(volumeType);
+    Volume volume0(volumeType, vs);
 
     VolumeSettings vsRot(vs);
     Matx44f pose;
     vsRot.getVolumePose(pose);
     pose = Affine3f(Vec3f(1, 1, 1), Vec3f()).matrix;
     vsRot.setVolumePose(pose);
-    Volume volumeRot(VolumeType::HashTSDF, vsRot);
+    Volume volumeRot(volumeType, vsRot);
 
     Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
     Matx33f intrIntegrate, intrRaycast;
@@ -796,8 +827,20 @@ void regressionVolPoseRot()
     UMat udepth;
     depth.copyTo(udepth);
 
-    volume0.integrate(udepth, poses[0].matrix);
-    volumeRot.integrate(udepth, poses[0].matrix);
+    const bool isColor = (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF);
+    UMat urgb;
+    if (isColor)
+    {
+        Mat rgb = scene->rgb(poses[0]);
+        rgb.copyTo(urgb);
+        volume0.integrate(udepth, urgb, poses[0].matrix);
+        volumeRot.integrate(udepth, urgb, poses[0].matrix);
+    }
+    else
+    {
+        volume0.integrate(udepth, poses[0].matrix);
+        volumeRot.integrate(udepth, poses[0].matrix);
+    }
 
     UMat upts, unrm, uptsRot, unrmRot;
 
@@ -875,15 +918,15 @@ namespace
 {
     struct VolumeTypeEnum
     {
-        static const std::array<VolumeType, 3> vals;
-        static const std::array<std::string, 3> svals;
+        static const std::array<VolumeType, 4> vals;
+        static const std::array<std::string, 4> svals;
 
         VolumeTypeEnum(VolumeType v = VolumeType::TSDF) : val(v) {}
         operator VolumeType() const { return val; }
         void PrintTo(std::ostream *os) const
         {
             int v = int(val);
-            if (v >= 0 && v < 3)
+            if (v >= 0 && v < 4)
             {
                 *os << svals[v];
             }
@@ -894,14 +937,24 @@ namespace
         }
         static ::testing::internal::ParamGenerator<VolumeTypeEnum> all()
         {
-            return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2]));
+            return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2]), VolumeTypeEnum(vals[3]));
         }
 
     private:
         VolumeType val;
     };
-    const std::array<VolumeType, 3> VolumeTypeEnum::vals{VolumeType::TSDF, VolumeType::HashTSDF, VolumeType::ColorTSDF};
-    const std::array<std::string, 3> VolumeTypeEnum::svals{std::string("TSDF"), std::string("HashTSDF"), std::string("ColorTSDF")};
+    const std::array<VolumeType, 4> VolumeTypeEnum::vals{
+        VolumeType::TSDF,
+        VolumeType::HashTSDF,
+        VolumeType::ColorTSDF,
+        VolumeType::ColorHashTSDF
+    };
+    const std::array<std::string, 4> VolumeTypeEnum::svals{
+        std::string("TSDF"),
+        std::string("HashTSDF"),
+        std::string("ColorTSDF"),
+        std::string("ColorHashTSDF")
+    };
 
     static inline void PrintTo(const VolumeTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
 
@@ -1011,7 +1064,7 @@ protected:
 
         if (testSrcType == VolumeTestSrcType::MAT)
         {
-            if (volumeType == VolumeType::ColorTSDF)
+            if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
                 volume->integrate(udepth, urgb, poses[0].matrix);
             else
                 volume->integrate(udepth, poses[0].matrix);
@@ -1056,7 +1109,8 @@ void VolumeTestFixture::saveObj(std::string funcName, Mat points, Mat normals)
     string platformString = gpu ? "GPU" : "CPU";
     string volumeTypeString = volumeType == VolumeType::TSDF ? "TSDF" :
                               volumeType == VolumeType::HashTSDF ? "HashTSDF" :
-                              volumeType == VolumeType::ColorTSDF  ? "ColorTSDF" : "";
+                              volumeType == VolumeType::ColorTSDF  ? "ColorTSDF" :
+                              volumeType == VolumeType::ColorHashTSDF  ? "ColorHashTSDF" : "";
     string testSrcTypeString = testSrcType == VolumeTestSrcType::MAT ? "MAT" :
                                testSrcType == VolumeTestSrcType::ODOMETRY_FRAME ? "OFRAME" : "";
     string frameSizeSpecifiedString = frameSizeSpecified == FrameSizeType::DEFAULT ? "DefaultSize" :
@@ -1071,14 +1125,14 @@ void VolumeTestFixture::raycast_test()
     UMat upoints, unormals, ucolors;
     if (frameSizeSpecified == FrameSizeType::CUSTOM)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors);
         else
             volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals);
     }
     else if (frameSizeSpecified == FrameSizeType::DEFAULT)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             volume->raycast(poses[0].matrix, upoints, unormals, ucolors);
         else
             volume->raycast(poses[0].matrix, upoints, unormals);
@@ -1091,7 +1145,7 @@ void VolumeTestFixture::raycast_test()
 
     if (cvtest::debugLevel > 0)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             displayColorImage(depth, rgb, points, normals, colors, depthFactor, lightPose);
         else
             displayImage(depth, points, normals, depthFactor, lightPose);
@@ -1146,14 +1200,14 @@ void VolumeTestFixture::valid_points_test()
     UMat upoints, unormals, ucolors;
     if (frameSizeSpecified == FrameSizeType::CUSTOM)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors);
         else
             volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals);
     }
     else if (frameSizeSpecified == FrameSizeType::DEFAULT)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             volume->raycast(poses[0].matrix, upoints, unormals, ucolors);
         else
             volume->raycast(poses[0].matrix, upoints, unormals);
@@ -1169,7 +1223,7 @@ void VolumeTestFixture::valid_points_test()
 
     if (cvtest::debugLevel > 0)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             displayColorImage(depth, rgb, points, normals, colors, depthFactor, lightPose);
         else
             displayImage(depth, points, normals, depthFactor, lightPose);
@@ -1180,14 +1234,14 @@ void VolumeTestFixture::valid_points_test()
 
     if (frameSizeSpecified == FrameSizeType::CUSTOM)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             volume->raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, upoints2, unormals2, ucolors2);
         else
             volume->raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, upoints2, unormals2);
     }
     else
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             volume->raycast(poses[17].matrix, upoints2, unormals2, ucolors2);
         else
             volume->raycast(poses[17].matrix, upoints2, unormals2);
@@ -1202,7 +1256,7 @@ void VolumeTestFixture::valid_points_test()
 
     if (cvtest::debugLevel > 0)
     {
-        if (volumeType == VolumeType::ColorTSDF)
+        if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF)
             displayColorImage(depth, rgb, points2, normals2, colors2, depthFactor, lightPose);
         else
             displayImage(depth, points2, normals2, depthFactor, lightPose);
@@ -1239,14 +1293,21 @@ TEST_P(VolumeTestFixture, fetch_normals)
 }
 
 //TODO: fix it when ColorTSDF gets GPU version
-INSTANTIATE_TEST_CASE_P(Volume, VolumeTestFixture, /*::testing::Combine(PlatformTypeEnum::all(), VolumeTypeEnum::all())*/
-                        ::testing::Combine(
-                        ::testing::Values(PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
-                                          PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF},
-                                          PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
-                                          PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF},
-                                          PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF}),
-                        VolumeTestSrcTypeEnum::all(), FrameSizeTypeEnum::all()));
+INSTANTIATE_TEST_CASE_P(Volume, VolumeTestFixture,
+    ::testing::Combine(
+        ::testing::Values(
+            PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
+            PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF},
+            PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
+            PlatformVolumeType {PlatformType::CPU, VolumeType::ColorHashTSDF},
+            PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF},
+            PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF}
+            // Note: Color types don't support GPU yet
+        ),
+        VolumeTestSrcTypeEnum::all(),
+        FrameSizeTypeEnum::all()
+    )
+);
 
 
 class StaticVolumeBoundingBox : public ::testing::TestWithParam<PlatformVolumeType>
@@ -1267,27 +1328,33 @@ TEST_P(StaticVolumeBoundingBox, staticBoundingBox)
 
 //TODO: edit this list when ColorTSDF gets GPU support
 INSTANTIATE_TEST_CASE_P(Volume, StaticVolumeBoundingBox, ::testing::Values(
-                        PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
-                        PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
-                        PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF}));
+    PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
+    PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
+    PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF}
+));
 
 
-class ReproduceVolPoseRotTest : public ::testing::TestWithParam<PlatformTypeEnum>
+class ReproduceVolPoseRotTest : public ::testing::TestWithParam<std::tuple<PlatformTypeEnum, VolumeTypeEnum>>
 { };
 
 TEST_P(ReproduceVolPoseRotTest, reproduce_volPoseRot)
 {
-    bool gpu = (GetParam() == PlatformType::GPU);
+    auto p = GetParam();
+    bool gpu = (std::get<0>(p) == PlatformType::GPU);
+    VolumeType volumeType = std::get<1>(p);
 
     OpenCLStatusRevert oclStatus;
 
     if (!gpu)
         oclStatus.off();
 
-    regressionVolPoseRot();
+    regressionVolPoseRot(volumeType);
 }
 
-INSTANTIATE_TEST_CASE_P(Volume, ReproduceVolPoseRotTest, PlatformTypeEnum::all());
+INSTANTIATE_TEST_CASE_P(Volume, ReproduceVolPoseRotTest,
+    ::testing::Combine(PlatformTypeEnum::all(),
+        ::testing::Values(VolumeTypeEnum(VolumeType::HashTSDF),
+                          VolumeTypeEnum(VolumeType::ColorHashTSDF))));
 
 
 enum Growth
@@ -1296,7 +1363,7 @@ enum Growth
 };
 CV_ENUM(GrowthEnum, Growth::OFF, Growth::ON);
 
-class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam<std::tuple<PlatformTypeEnum, GrowthEnum>>
+class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam<std::tuple<PlatformTypeEnum, GrowthEnum, VolumeTypeEnum>>
 { };
 
 TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth)
@@ -1304,35 +1371,43 @@ TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth)
     auto p = GetParam();
     bool gpu = (std::get<0>(p) == PlatformType::GPU);
     bool enableGrowth = (std::get<1>(p) == Growth::ON);
+    VolumeType volumeType = std::get<2>(p);
 
     OpenCLStatusRevert oclStatus;
 
     if (!gpu)
         oclStatus.off();
 
-    boundingBoxGrowthTest(enableGrowth);
+    boundingBoxGrowthTest(volumeType, enableGrowth);
 }
 
-INSTANTIATE_TEST_CASE_P(Volume, BoundingBoxEnableGrowthTest, ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all()));
+INSTANTIATE_TEST_CASE_P(Volume, BoundingBoxEnableGrowthTest,
+    ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all(),
+        ::testing::Values(VolumeTypeEnum(VolumeType::HashTSDF),
+                          VolumeTypeEnum(VolumeType::ColorHashTSDF))));
 
 
-class HugeSceneGrowthTest : public ::testing::TestWithParam<PlatformTypeEnum>
+class HugeSceneGrowthTest : public ::testing::TestWithParam<std::tuple<PlatformTypeEnum, VolumeTypeEnum>>
 { };
 
 TEST_P(HugeSceneGrowthTest, boundingBoxEnableGrowth)
 {
     auto p = GetParam();
-    bool gpu = (p == PlatformType::GPU);
+    bool gpu = (std::get<0>(p) == PlatformType::GPU);
+    VolumeType volumeType = std::get<1>(p);
 
     OpenCLStatusRevert oclStatus;
 
     if (!gpu)
         oclStatus.off();
 
-    hugeSceneGrowthTest();
+    hugeSceneGrowthTest(volumeType);
 }
 
-INSTANTIATE_TEST_CASE_P(Volume, HugeSceneGrowthTest, PlatformTypeEnum::all());
+INSTANTIATE_TEST_CASE_P(Volume, HugeSceneGrowthTest,
+    ::testing::Combine(PlatformTypeEnum::all(),
+        ::testing::Values(VolumeTypeEnum(VolumeType::HashTSDF),
+                          VolumeTypeEnum(VolumeType::ColorHashTSDF))));
 
 }
 }  // namespace