New HashTSDFVolumeGPU class

modules/rgbd/samples/large_kinfu_demo.cpp

@@ -127,8 +127,7 @@ int main(int argc, char** argv)
     // These params can be different for each depth sensor
     ds->updateParams(*params);
 
-    // Disabled until there is no OpenCL accelerated HashTSDF is available
-    cv::setUseOptimized(false);
+    cv::setUseOptimized(true);
 
     if (!idle)
         largeKinfu = LargeKinfu::create(params);

modules/rgbd/src/hash_tsdf.hpp

@@ -26,108 +26,26 @@ class HashTSDFVolume : public Volume
 
     virtual ~HashTSDFVolume() = default;
 
+    virtual int getVisibleBlocks(int currFrameId, int frameThreshold) const = 0;
+    virtual size_t getTotalVolumeUnits() const = 0;
+
    public:
     int maxWeight;
     float truncDist;
     float truncateThreshold;
     int volumeUnitResolution;
+    int volumeUnitDegree;
     float volumeUnitSize;
     bool zFirstMemOrder;
+    Vec4i volStrides;
 };
 
-//! Spatial hashing
-struct tsdf_hash
-{
-    size_t operator()(const Vec3i& x) const noexcept
-    {
-        size_t seed                     = 0;
-        constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
-        for (uint16_t i = 0; i < 3; i++)
-        {
-            seed ^= std::hash<int>()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
-        }
-        return seed;
-    }
-};
-
-typedef unsigned int VolumeIndex;
-struct VolumeUnit
-{
-    cv::Vec3i coord;
-    VolumeIndex index;
-    cv::Matx44f pose;
-    int lastVisibleIndex = 0;
-    bool isActive;
-};
-
-typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
-typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
-
-class HashTSDFVolumeCPU : public HashTSDFVolume
-{
-   public:
-    // dimension in voxels, size in meters
-    HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight,
-                      float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true);
-
-    HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true);
-
-    void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics,
-                    const int frameId = 0) override;
-    void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points,
-                  OutputArray normals) const override;
-
-    void fetchNormals(InputArray points, OutputArray _normals) const override;
-    void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
-
-    void reset() override;
-    size_t getTotalVolumeUnits() const { return volumeUnits.size(); }
-    int getVisibleBlocks(int currFrameId, int frameThreshold) const;
-
-    //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length)
-    TsdfVoxel at(const Vec3i& volumeIdx) const;
-
-    //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit =
-    //! 1m)
-    virtual TsdfVoxel at(const cv::Point3f& point) const;
-    virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const;
-
-    TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const;
-
-
-    float interpolateVoxelPoint(const Point3f& point) const;
-    float interpolateVoxel(const cv::Point3f& point) const;
-    Point3f getNormalVoxel(const cv::Point3f& p) const;
-
-    //! Utility functions for coordinate transformations
-    Vec3i volumeToVolumeUnitIdx(const Point3f& point) const;
-    Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const;
-
-    Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const;
-    Vec3i volumeToVoxelCoord(const Point3f& point) const;
-
-   public:
-       Vec4i volStrides;
-       Vec6f frameParams;
-       Mat pixNorms;
-       VolumeUnitIndexes volumeUnits;
-       cv::Mat volUnitsData;
-       VolumeIndex lastVolIndex;
-};
-
-template<typename T>
-Ptr<HashTSDFVolume> makeHashTSDFVolume(const VolumeParams& _volumeParams)
-{
-    return makePtr<T>(_volumeParams);
-}
-
-template<typename T>
+//template<typename T>
+Ptr<HashTSDFVolume> makeHashTSDFVolume(const VolumeParams& _volumeParams);
+//template<typename T>
 Ptr<HashTSDFVolume> makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
-                                          int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16)
-{
-    return makePtr<T>(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold,
-                      volumeUnitResolution);
-}
+    int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16);
+
 }  // namespace kinfu
 }  // namespace cv
 #endif

modules/rgbd/src/large_kinfu.cpp

@@ -316,21 +316,21 @@ template<typename MatType>
 void LargeKinfuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
 {
     auto currSubmap = submapMgr->getCurrentSubmap();
-    currSubmap->volume.fetchPointsNormals(p, n);
+    currSubmap->volume->fetchPointsNormals(p, n);
 }
 
 template<typename MatType>
 void LargeKinfuImpl<MatType>::getPoints(OutputArray points) const
 {
     auto currSubmap = submapMgr->getCurrentSubmap();
-    currSubmap->volume.fetchPointsNormals(points, noArray());
+    currSubmap->volume->fetchPointsNormals(points, noArray());
 }
 
 template<typename MatType>
 void LargeKinfuImpl<MatType>::getNormals(InputArray points, OutputArray normals) const
 {
     auto currSubmap = submapMgr->getCurrentSubmap();
-    currSubmap->volume.fetchNormals(points, normals);
+    currSubmap->volume->fetchNormals(points, normals);
 }
 
 // importing class