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

modules/rgbd/CMakeLists.txt

@@ -1,2 +1,11 @@
 set(the_description "RGBD algorithms")
+
+find_package(Ceres QUIET)
 ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
+ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES})
+
+if(Ceres_FOUND)
+  ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND)
+else()
+  message(STATUS "CERES support is disabled. Ceres Solver is Required for Posegraph optimization")
+endif()

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/dynafu.hpp

@@ -10,103 +10,11 @@
 #include "opencv2/core.hpp"
 #include "opencv2/core/affine.hpp"
 
+#include "kinfu.hpp"
+
 namespace cv {
 namespace dynafu {
 
-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 frame size in pixels */
-    CV_PROP_RW Size frameSize;
-
-    /** @brief camera intrinsics */
-    CV_PROP 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 Resolution of voxel space
-
-    Number of voxels in each dimension.
-    */
-    CV_PROP_RW Vec3i volumeDims;
-    /** @brief Size of voxel in meters */
-    CV_PROP_RW float voxelSize;
-
-    /** @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 initial volume pose in meters */
-    Affine3f volumePose;
-
-    /** @brief distance to truncate in meters
-
-    Distances to surface that exceed this value will be truncated to 1.0.
-    */
-    CV_PROP_RW float tsdf_trunc_dist;
-
-    /** @brief max number of frames per voxel
-
-    Each voxel keeps running average of distances no longer than this value.
-    */
-    CV_PROP_RW int tsdf_max_weight;
-
-    /** @brief A length of one raycast step
-
-    How much voxel sizes we skip each raycast step
-    */
-    CV_PROP_RW float raycast_step_factor;
-
-    // gradient delta in voxel sizes
-    // fixed at 1.0f
-    // float gradient_delta_factor;
-
-    /** @brief light pose for rendering in meters */
-    CV_PROP Vec3f lightPose;
-
-    /** @brief distance theshold for ICP in meters */
-    CV_PROP_RW float icpDistThresh;
-    /** angle threshold for ICP in radians */
-    CV_PROP_RW float icpAngleThresh;
-    /** number of ICP iterations for each pyramid level */
-    CV_PROP 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 DynamicFusion implementation
 
   This class implements a 3d reconstruction algorithm as described in @cite dynamicfusion.
@@ -132,11 +40,11 @@ struct CV_EXPORTS_W Params
 class CV_EXPORTS_W DynaFu
 {
 public:
-    CV_WRAP static Ptr<DynaFu> create(const Ptr<Params>& _params);
+    CV_WRAP static Ptr<DynaFu> create(const Ptr<kinfu::Params>& _params);
     virtual ~DynaFu();
 
     /** @brief Get current parameters */
-    virtual const Params& getParams() const = 0;
+    virtual const kinfu::Params& getParams() const = 0;
 
     /** @brief Renders a volume into an image
 

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

@@ -24,22 +24,22 @@ struct CV_EXPORTS_W Params
     /**
      * @brief Constructor for Params
      * Sets the initial pose of the TSDF volume.
-     * @param volumeIntialPoseRot rotation matrix
-     * @param volumeIntialPoseTransl translation vector
+     * @param volumeInitialPoseRot rotation matrix
+     * @param volumeInitialPoseTransl translation vector
      */
-    CV_WRAP Params(Matx33f volumeIntialPoseRot, Vec3f volumeIntialPoseTransl)
+    CV_WRAP Params(Matx33f volumeInitialPoseRot, Vec3f volumeInitialPoseTransl)
     {
-      setInitialVolumePose(volumeIntialPoseRot,volumeIntialPoseTransl);
+      setInitialVolumePose(volumeInitialPoseRot,volumeInitialPoseTransl);
     }
 
     /**
      * @brief Constructor for Params
      * Sets the initial pose of the TSDF volume.
-     * @param volumeIntialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume
+     * @param volumeInitialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume
      */
-    CV_WRAP Params(Matx44f volumeIntialPose)
+    CV_WRAP Params(Matx44f volumeInitialPose)
     {
-      setInitialVolumePose(volumeIntialPose);
+      setInitialVolumePose(volumeInitialPose);
     }
 
     /**
@@ -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,143 @@
+// 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 Large Scale Dense Depth Fusion implementation
+
+  This class implements a 3d reconstruction algorithm for larger environments using
+  Spatially hashed TSDF volume "Submaps".
+  It also runs a periodic posegraph optimization to minimize drift in tracking over long sequences.
+  Currently the algorithm does not implement a relocalization or loop closure module.
+  Potentially a Bag of words implementation or RGBD relocalization as described in
+  Glocker et al. ISMAR 2013 will be implemented
+
+  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 spatially hashed voxel cube that stores TSDF values
+  which represent the distance to the closest surface (for details read the @cite kinectfusion article
+  about TSDF). There is no interface to that representation yet.
+
+  For posegraph optimization, a Submap abstraction over the Volume class is created.
+  New submaps are added to the model when there is low visibility overlap between current viewing frustrum
+  and the existing volume/model. Multiple submaps are simultaneously tracked and a posegraph is created and
+  optimized periodically.
+
+  LargeKinfu does not use any OpenCL acceleration yet.
+  To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL().
+
+  This implementation is inspired from Kintinuous, InfiniTAM and other SOTA algorithms
+
+  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