opencv
diff --git a/‎modules/rgbd/include/opencv2/rgbd/intrinsics.hpp
Copy file name to clipboard
+81Lines changed: 81 additions & 0 deletions b/‎modules/rgbd/include/opencv2/rgbd/intrinsics.hpp
Copy file name to clipboard
+81Lines changed: 81 additions & 0 deletions
diff --git a/‎modules/rgbd/include/opencv2/rgbd/kinfu.hpp
Copy file name to clipboard
+8Lines changed: 8 additions & 0 deletions b/‎modules/rgbd/include/opencv2/rgbd/kinfu.hpp
Copy file name to clipboard
+8Lines changed: 8 additions & 0 deletions
diff --git a/‎modules/rgbd/include/opencv2/rgbd/volume.hpp
Copy file name to clipboard
+58Lines changed: 58 additions & 0 deletions b/‎modules/rgbd/include/opencv2/rgbd/volume.hpp
Copy file name to clipboard
+58Lines changed: 58 additions & 0 deletions
diff --git a/‎modules/rgbd/samples/kinfu_demo.cpp
Copy file name to clipboard
+9Lines changed: 9 additions & 0 deletions b/‎modules/rgbd/samples/kinfu_demo.cpp
Copy file name to clipboard
+9Lines changed: 9 additions & 0 deletions
@@ -0,0 +1,81 @@
+// 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 https://opencv.org/license.html
+
+#ifndef __OPENCV_RGBD_INTRINSICS_HPP__
+#define __OPENCV_RGBD_INTRINSICS_HPP__
+
+#include "opencv2/core/matx.hpp"
+
+namespace cv
+{
+namespace kinfu
+{
+
+struct Intr
+{
+    /** @brief Camera intrinsics */
+    /** Reprojects screen point to camera space given z coord. */
+    struct Reprojector
+    {
+        Reprojector() {}
+        inline Reprojector(Intr intr)
+        {
+            fxinv = 1.f/intr.fx, fyinv = 1.f/intr.fy;
+            cx = intr.cx, cy = intr.cy;
+        }
+        template<typename T>
+        inline cv::Point3_<T> operator()(cv::Point3_<T> p) const
+        {
+            T x = p.z * (p.x - cx) * fxinv;
+            T y = p.z * (p.y - cy) * fyinv;
+            return cv::Point3_<T>(x, y, p.z);
+        }
+
+        float fxinv, fyinv, cx, cy;
+    };
+
+    /** Projects camera space vector onto screen */
+    struct Projector
+    {
+        inline Projector(Intr intr) : fx(intr.fx), fy(intr.fy), cx(intr.cx), cy(intr.cy) { }
+        template<typename T>
+        inline cv::Point_<T> operator()(cv::Point3_<T> p) const
+        {
+            T invz = T(1)/p.z;
+            T x = fx*(p.x*invz) + cx;
+            T y = fy*(p.y*invz) + cy;
+            return cv::Point_<T>(x, y);
+        }
+        template<typename T>
+        inline cv::Point_<T> operator()(cv::Point3_<T> p, cv::Point3_<T>& pixVec) const
+        {
+            T invz = T(1)/p.z;
+            pixVec = cv::Point3_<T>(p.x*invz, p.y*invz, 1);
+            T x = fx*pixVec.x + cx;
+            T y = fy*pixVec.y + cy;
+            return cv::Point_<T>(x, y);
+        }
+        float fx, fy, cx, cy;
+    };
+    Intr() : fx(), fy(), cx(), cy() { }
+    Intr(float _fx, float _fy, float _cx, float _cy) : fx(_fx), fy(_fy), cx(_cx), cy(_cy) { }
+    Intr(cv::Matx33f m) : fx(m(0, 0)), fy(m(1, 1)), cx(m(0, 2)), cy(m(1, 2)) { }
+    // scale intrinsics to pyramid level
+    inline Intr scale(int pyr) const
+    {
+        float factor = (1.f /(1 << pyr));
+        return Intr(fx*factor, fy*factor, cx*factor, cy*factor);
+    }
+    inline Reprojector makeReprojector() const { return Reprojector(*this); }
+    inline Projector   makeProjector()   const { return Projector(*this);   }
+
+    inline cv::Matx33f getMat() const { return Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); }
+
+    float fx, fy, cx, cy;
+};
+
+} // namespace rgbd
+} // namespace cv
+
+#endif
@@ -9,6 +9,7 @@
 
 #include "opencv2/core.hpp"
 #include "opencv2/core/affine.hpp"
+#include <opencv2/rgbd/volume.hpp>
 
 namespace cv {
 namespace kinfu {
@@ -68,9 +69,16 @@ struct CV_EXPORTS_W Params
     */
     CV_WRAP static Ptr<Params> coarseParams();
 
+    /** @brief HashTSDF parameters
+      A set of parameters suitable for use with HashTSDFVolume
+    */
+    CV_WRAP static Ptr<Params> hashTSDFParams(bool isCoarse);
+
     /** @brief frame size in pixels */
     CV_PROP_RW Size frameSize;
 
+    CV_PROP_RW cv::kinfu::VolumeType volumeType;
+
     /** @brief camera intrinsics */
     CV_PROP_RW Matx33f intr;
 
 
@@ -0,0 +1,58 @@
+// 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 https://opencv.org/license.html
+
+// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
+// module's directory
+
+#ifndef __OPENCV_RGBD_VOLUME_H__
+#define __OPENCV_RGBD_VOLUME_H__
+
+#include "intrinsics.hpp"
+#include "opencv2/core/affine.hpp"
+
+namespace cv
+{
+namespace kinfu
+{
+class Volume
+{
+   public:
+    Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor)
+        : voxelSize(_voxelSize),
+          voxelSizeInv(1.0f / voxelSize),
+          pose(_pose),
+          raycastStepFactor(_raycastStepFactor)
+    {
+    }
+
+    virtual ~Volume(){};
+
+    virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose,
+                           const cv::kinfu::Intr& intrinsics)                        = 0;
+    virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics,
+                         cv::Size frameSize, cv::OutputArray points,
+                         cv::OutputArray normals) const                                    = 0;
+    virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const       = 0;
+    virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0;
+    virtual void reset()                                                                   = 0;
+
+   public:
+    const float voxelSize;
+    const float voxelSizeInv;
+    const cv::Affine3f pose;
+    const float raycastStepFactor;
+};
+
+enum class VolumeType
+{
+    TSDF     = 0,
+    HASHTSDF = 1
+};
+
+cv::Ptr<Volume> makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose,
+                           float _raycastStepFactor, float _truncDist, int _maxWeight,
+                           float _truncateThreshold, Point3i _resolution);
+}  // namespace kinfu
+}  // namespace cv
+#endif
@@ -312,6 +312,7 @@ static const char* keys =
     "{camera |0| Index of depth camera to be used as a depth source }"
     "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
         " in coarse mode points and normals are displayed }"
+    "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}"
     "{idle   | | Do not run KinFu, just display depth frames }"
     "{record | | Write depth frames to specified file list"
         " (the same format as for the 'depth' key) }"
@@ -327,6 +328,7 @@ int main(int argc, char **argv)
 {
     bool coarse = false;
     bool idle = false;
+    bool useHashTSDF = false;
     string recordPath;
 
     CommandLineParser parser(argc, argv, keys);
@@ -352,6 +354,10 @@ int main(int argc, char **argv)
     {
         recordPath = parser.get<String>("record");
     }
+    if(parser.has("useHashTSDF"))
+    {
+        useHashTSDF = true;
+    }
     if(parser.has("idle"))
     {
         idle = true;
@@ -382,6 +388,9 @@ int main(int argc, char **argv)
     else
         params = Params::defaultParams();
 
+    if(useHashTSDF)
+        params = Params::hashTSDFParams(coarse);
+
     // These params can be different for each depth sensor
     ds->updateParams(*params);