[GSoC] Add Submaps and PoseGraph optimization for Large Scale Depth Fusion

modules/rgbd/include/opencv2/rgbd.hpp

@@ -13,6 +13,7 @@
 #include "opencv2/rgbd/depth.hpp"
 #include "opencv2/rgbd/kinfu.hpp"
 #include "opencv2/rgbd/dynafu.hpp"
+#include "opencv2/rgbd/large_kinfu.hpp"
 
 
 /** @defgroup rgbd RGB-Depth Processing

modules/rgbd/include/opencv2/rgbd/kinfu.hpp

@@ -77,7 +77,7 @@ struct CV_EXPORTS_W Params
     /** @brief frame size in pixels */
     CV_PROP_RW Size frameSize;
 
-    CV_PROP_RW cv::kinfu::VolumeType volumeType;
+    CV_PROP_RW kinfu::VolumeType volumeType;
 
     /** @brief camera intrinsics */
     CV_PROP_RW Matx33f intr;

modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp

@@ -0,0 +1,137 @@
+// 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
+
+// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
+// module's directory
+
+#ifndef __OPENCV_RGBD_LARGEKINFU_HPP__
+#define __OPENCV_RGBD_LARGEKINFU_HPP__
+
+#include <opencv2/rgbd/volume.hpp>
+
+#include "opencv2/core.hpp"
+#include "opencv2/core/affine.hpp"
+
+namespace cv
+{
+namespace large_kinfu
+{
+struct CV_EXPORTS_W Params
+{
+    /** @brief Default parameters
+        A set of parameters which provides better model quality, can be very slow.
+     */
+    CV_WRAP static Ptr<Params> defaultParams();
+
+    /** @brief Coarse parameters
+        A set of parameters which provides better speed, can fail to match frames
+        in case of rapid sensor motion.
+    */
+    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;
+
+    /** @brief camera intrinsics */
+    CV_PROP_RW Matx33f intr;
+
+    /** @brief pre-scale per 1 meter for input values
+        Typical values are:
+             * 5000 per 1 meter for the 16-bit PNG files of TUM database
+             * 1000 per 1 meter for Kinect 2 device
+             * 1 per 1 meter for the 32-bit float images in the ROS bag files
+    */
+    CV_PROP_RW float depthFactor;
+
+    /** @brief Depth sigma in meters for bilateral smooth */
+    CV_PROP_RW float bilateral_sigma_depth;
+    /** @brief Spatial sigma in pixels for bilateral smooth */
+    CV_PROP_RW float bilateral_sigma_spatial;
+    /** @brief Kernel size in pixels for bilateral smooth */
+    CV_PROP_RW int bilateral_kernel_size;
+
+    /** @brief Number of pyramid levels for ICP */
+    CV_PROP_RW int pyramidLevels;
+
+    /** @brief Minimal camera movement in meters
+        Integrate new depth frame only if camera movement exceeds this value.
+    */
+    CV_PROP_RW float tsdf_min_camera_movement;
+
+    /** @brief light pose for rendering in meters */
+    CV_PROP_RW Vec3f lightPose;
+
+    /** @brief distance theshold for ICP in meters */
+    CV_PROP_RW float icpDistThresh;
+    /** @brief angle threshold for ICP in radians */
+    CV_PROP_RW float icpAngleThresh;
+    /** @brief number of ICP iterations for each pyramid level */
+    CV_PROP_RW std::vector<int> icpIterations;
+
+    /** @brief Threshold for depth truncation in meters
+        All depth values beyond this threshold will be set to zero
+    */
+    CV_PROP_RW float truncateThreshold;
+
+    /** @brief Volume parameters
+    */
+    kinfu::VolumeParams volumeParams;
+};
+
+/** @brief KinectFusion implementation
+
+  This class implements a 3d reconstruction algorithm described in
+  @cite kinectfusion paper.
+
+  It takes a sequence of depth images taken from depth sensor
+  (or any depth images source such as stereo camera matching algorithm or even raymarching
+  renderer). The output can be obtained as a vector of points and their normals or can be
+  Phong-rendered from given camera pose.
+
+  An internal representation of a model is a voxel cuboid that keeps TSDF values
+  which are a sort of distances to the surface (for details read the @cite kinectfusion article
+  about TSDF). There is no interface to that representation yet.
+
+  LargeKinfu uses OpenCL acceleration automatically if available.
+  To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL().
+
+  This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake).
+
+  Note that the KinectFusion algorithm was patented and its use may be restricted by
+  the list of patents mentioned in README.md file in this module directory.
+
+  That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion.
+*/
+class CV_EXPORTS_W LargeKinfu
+{
+   public:
+    CV_WRAP static Ptr<LargeKinfu> create(const Ptr<Params>& _params);
+    virtual ~LargeKinfu() = default;
+
+    virtual const Params& getParams() const = 0;
+
+    CV_WRAP virtual void render(OutputArray image,
+                                const Matx44f& cameraPose = Matx44f::eye()) const = 0;
+
+    CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
+
+    CV_WRAP virtual void getPoints(OutputArray points) const = 0;
+
+    CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0;
+
+    CV_WRAP virtual void reset() = 0;
+
+    virtual const Affine3f getPose() const = 0;
+
+    CV_WRAP virtual bool update(InputArray depth) = 0;
+};
+
+}  // namespace large_kinfu
+}  // namespace cv
+#endif

modules/rgbd/include/opencv2/rgbd/volume.hpp

@@ -19,20 +19,16 @@ class Volume
 {
    public:
     Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor)
-        : voxelSize(_voxelSize),
-          voxelSizeInv(1.0f / voxelSize),
-          pose(_pose),
-          raycastStepFactor(_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;
+                           const cv::kinfu::Intr& intrinsics, const int frameId = 0)       = 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;
@@ -50,9 +46,65 @@ enum class VolumeType
     HASHTSDF = 1
 };
 
-cv::Ptr<Volume> makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose,
-                           float _raycastStepFactor, float _truncDist, int _maxWeight,
-                           float _truncateThreshold, Point3i _resolution);
+struct VolumeParams
+{
+    /** @brief Type of Volume
+        Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
+    */
+    VolumeType type;
+
+    /** @brief Resolution of voxel space
+        Number of voxels in each dimension.
+    */
+    Vec3i resolution;
+
+    /** @brief Resolution of volumeUnit in voxel space
+        Number of voxels in each dimension for volumeUnit
+        Applicable only for hashTSDF.
+    */
+    int unitResolution = 0;
+
+    /** @brief Initial pose of the volume in meters */
+    cv::Affine3f pose;
+
+    /** @brief Length of voxels in meters */
+    float voxelSize;
+
+    /** @brief TSDF truncation distance
+        Distances greater than value from surface will be truncated to 1.0
+    */
+    float tsdfTruncDist;
+
+    /** @brief Max number of frames to integrate per voxel
+        Each voxel stops integration after the maxWeight is crossed
+    */
+    int maxWeight;
+
+    /** @brief Threshold for depth truncation in meters
+        Truncates the depth greater than threshold to 0
+    */
+    float depthTruncThreshold;
+
+    /** @brief Length of single raycast step
+        Describes the percentage of voxel length that is skipped per march
+    */
+    float raycastStepFactor;
+
+    /** @brief Default set of parameters that provide higher quality reconstruction
+        at the cost of slow performance.
+    */
+    static Ptr<VolumeParams> defaultParams(VolumeType _volumeType);
+
+    /** @brief Coarse set of parameters that provides relatively higher performance
+        at the cost of reconstrution quality.
+    */
+    static Ptr<VolumeParams> coarseParams(VolumeType _volumeType);
+};
+
+cv::Ptr<Volume> makeVolume(const VolumeParams& _volumeParams);
+
+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