Merge pull request #21189 from DumDereDum:new_volume

New Volume pipeline

modules/3d/include/opencv2/3d/detail/submap.hpp

@@ -43,25 +43,25 @@ public:
     };
     typedef std::map<int, PoseConstraint> Constraints;
 
-    Submap(int _id, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(),
+    Submap(int _id, const VolumeSettings& settings, const cv::Affine3f& _pose = cv::Affine3f::Identity(),
            int _startFrameId = 0)
         : id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId)
     {
-        VolumeParams vp = volumeParams;
-        vp.kind = VolumeParams::VolumeKind::HASHTSDF;
-        Ptr<VolumeParams> pvp = makePtr<VolumeParams>(vp);
-        volume = makeVolume(pvp);
+        volume = Volume(VolumeType::HashTSDF, settings);
     }
     virtual ~Submap() = default;
 
-    virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics, const int currframeId);
-    virtual void raycast(const Odometry& icp, const cv::Affine3f& cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
+    virtual void integrate(InputArray _depth, const int currframeId);
+    virtual void raycast(const Odometry& icp, const cv::Affine3f& cameraPose, cv::Size frameSize,
                          OutputArray points = noArray(), OutputArray normals = noArray());
 
-    virtual int getTotalAllocatedBlocks() const { return int(volume->getTotalVolumeUnits()); };
+    virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); };
     virtual int getVisibleBlocks(int currFrameId) const
     {
-        return volume->getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
+        CV_Assert(currFrameId >= startFrameId);
+        //return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
+        return volume.getVisibleBlocks();
+
     }
 
     float calcVisibilityRatio(int currFrameId) const
@@ -100,20 +100,19 @@ public:
     OdometryFrame frame;
     OdometryFrame renderFrame;
 
-    std::shared_ptr<Volume> volume;
+    Volume volume;
 };
 
 template<typename MatType>
 
-void Submap<MatType>::integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics,
-                                const int currFrameId)
+void Submap<MatType>::integrate(InputArray _depth, const int currFrameId)
 {
     CV_Assert(currFrameId >= startFrameId);
-    volume->integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId);
+    volume.integrate(_depth, cameraPose.matrix);
 }
 
 template<typename MatType>
-void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
+void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPose, cv::Size frameSize,
                               OutputArray points, OutputArray normals)
 {
     if (!points.needed() && !normals.needed())
@@ -122,20 +121,20 @@ void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPo
 
         frame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
         frame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
-        volume->raycast(_cameraPose.matrix, intrinsics, frameSize, pts, nrm);
+        volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, pts, nrm);
         frame.setPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
         frame.setPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM,  0);
 
         renderFrame = frame;
 
         Mat depth;
-        frame.getDepth(depth);
+        frame.getScaledDepth(depth);
         frame = icp.createOdometryFrame();
         frame.setDepth(depth);
     }
     else
     {
-        volume->raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals);
+        volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, points, normals);
     }
 }
 
@@ -188,7 +187,7 @@ public:
     typedef std::map<int, Ptr<SubmapT>> IdToSubmapPtr;
     typedef std::unordered_map<int, ActiveSubmapData> IdToActiveSubmaps;
 
-    SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {}
+    explicit SubmapManager(const VolumeSettings& _volumeSettings) : volumeSettings(_volumeSettings) {}
     virtual ~SubmapManager() = default;
 
     void reset() { submapList.clear(); };
@@ -214,7 +213,7 @@ public:
     Ptr<detail::PoseGraph> MapToPoseGraph();
     void PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph);
 
-    VolumeParams volumeParams;
+    VolumeSettings volumeSettings;
 
     std::vector<Ptr<SubmapT>> submapList;
     IdToActiveSubmaps activeSubmaps;
@@ -227,7 +226,7 @@ int SubmapManager<MatType>::createNewSubmap(bool isCurrentMap, int currFrameId,
 {
     int newId = int(submapList.size());
 
-    Ptr<SubmapT> newSubmap = cv::makePtr<SubmapT>(newId, volumeParams, pose, currFrameId);
+    Ptr<SubmapT> newSubmap = cv::makePtr<SubmapT>(newId, volumeSettings, pose, currFrameId);
     submapList.push_back(newSubmap);
 
     ActiveSubmapData newSubmapData;

modules/3d/include/opencv2/3d/odometry_frame.hpp

@@ -52,6 +52,7 @@ public:
     void getGrayImage(OutputArray image) const;
     void setDepth(InputArray  depth);
     void getDepth(OutputArray depth) const;
+    void getScaledDepth(OutputArray depth) const;
     void setMask(InputArray  mask);
     void getMask(OutputArray mask) const;
     void setNormals(InputArray  normals);

modules/3d/include/opencv2/3d/volume.hpp

@@ -5,124 +5,135 @@
 #ifndef OPENCV_3D_VOLUME_HPP
 #define OPENCV_3D_VOLUME_HPP
 
+#include "volume_settings.hpp"
+
 #include "opencv2/core/affine.hpp"
 
 namespace cv
 {
+
 class CV_EXPORTS_W Volume
 {
-   public:
-    Volume(float _voxelSize, Matx44f _pose, float _raycastStepFactor) :
-        voxelSize(_voxelSize),
-        voxelSizeInv(1.0f / voxelSize),
-        pose(_pose),
-        raycastStepFactor(_raycastStepFactor)
-    { }
+public:
+    /** @brief Constructor of default volume - TSDF.
+    */
+    Volume();
+    /** @brief Constructor of custom volume.
+    * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
+    * @param settings the custom settings for volume.
+    */
+    Volume(VolumeType vtype, const VolumeSettings& settings);
+    ~Volume();
 
-    virtual ~Volume(){};
+    /** @brief Integrates the input data to the volume.
 
-    CV_WRAP
-    virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose,
-                           const Matx33f& intrinsics, const int frameId = 0)                   = 0;
-    virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor,
-                           const Matx44f& cameraPose, const Matx33f& intrinsics,
-                           const Matx33f& rgb_intrinsics, const int frameId = 0)               = 0;
-    CV_WRAP
-    virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics,
-                         const Size& frameSize, OutputArray points, OutputArray normals) const = 0;
-    virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics, const Size& frameSize,
-                         OutputArray points, OutputArray normals, OutputArray colors) const    = 0;
-    CV_WRAP
-    virtual void fetchNormals(InputArray points, OutputArray _normals) const                   = 0;
-    CV_WRAP
-    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const             = 0;
-    CV_WRAP
-    virtual void reset()                                                                       = 0;
+    Camera intrinsics are taken from volume settings structure.
 
-    // Works for hash-based volumes only, otherwise returns 1
-    virtual int getVisibleBlocks(int /*currFrameId*/, int /*frameThreshold*/) const { return 1; }
-    virtual size_t getTotalVolumeUnits() const { return 1; }
+    * @param frame the object from which to take depth and image data.
+      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.
+    */
+    void integrate(const OdometryFrame& frame, InputArray pose);
 
-   public:
-    const float voxelSize;
-    const float voxelSizeInv;
-    const Affine3f pose;
-    const float raycastStepFactor;
+    /** @brief Integrates the input data to the volume.
+
+    Camera intrinsics are taken from volume settings structure.
+
+    * @param depth the depth image.
+    * @param pose the pose of camera in global coordinates.
+    */
+    void integrate(InputArray depth, InputArray pose);
+
+    /** @brief Integrates the input data to the volume.
+
+    Camera intrinsics are taken from volume settings structure.
+
+    * @param depth the depth image.
+    * @param image the color image (only for ColorTSDF).
+      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.
+    */
+    void integrate(InputArray depth, InputArray image, InputArray pose);
+
+    /** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
+
+    Rendered image size and camera intrinsics are taken from volume settings structure.
+
+    * @param cameraPose the pose of camera in global coordinates.
+    * @param outFrame the object where to store rendered points and normals.
+    */
+    void raycast(InputArray cameraPose, OdometryFrame& outFrame) const;
+
+    /** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
+
+    Rendered image size and camera intrinsics are taken from volume settings structure.
+
+    * @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).
+    */
+    void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
+
+    /** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
+
+    Rendered image size and camera intrinsics are taken from volume settings structure.
+
+    * @param cameraPose the pose of camera in global coordinates.
+    * @param height the height of result image.
+    * @param width the width of result image.
+    * @param outFrame the object where to store rendered points and normals.
+    */
+    void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const;
+
+    /** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
+
+    Rendered image size and camera intrinsics are taken from volume settings structure.
+
+    * @param cameraPose the pose of camera in global coordinates.
+    * @param height the height of result image.
+    * @param width the width of result image.
+    * @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).
+    */
+    void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
+
+    /** @brief Extract the all data from volume.
+    * @param points the input exist point.
+    * @param normals the storage of normals (corresponding to input points) in the image.
+    */
+    void fetchNormals(InputArray points, OutputArray normals) const;
+    /** @brief Extract the all data from volume.
+    * @param points the storage of all points.
+    * @param normals the storage of all normals, corresponding to points.
+    */
+    void fetchPointsNormals(OutputArray points, OutputArray normals) const;
+    /** @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).
+    */
+    void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const;
+
+    /** @brief clear all data in volume.
+    */
+    void reset();
+
+    /** @brief return visible blocks in volume.
+    */
+    int getVisibleBlocks() const;
+
+    /** @brief return number of vulmeunits in volume.
+    */
+    size_t getTotalVolumeUnits() const;
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
 };
 
-struct CV_EXPORTS_W VolumeParams
-{
-    enum VolumeKind
-    {
-        TSDF        = 0,
-        HASHTSDF    = 1,
-        COLOREDTSDF = 2
-    };
-
-    /** @brief Kind of Volume
-        Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
-    */
-    CV_PROP_RW int kind;
-
-    /** @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
-    */
-    CV_PROP_RW int resolutionX;
-    CV_PROP_RW int resolutionY;
-    CV_PROP_RW int resolutionZ;
-
-    /** @brief Resolution of volumeUnit in voxel space
-        Number of voxels in each dimension for volumeUnit
-        Applicable only for hashTSDF.
-    */
-    CV_PROP_RW int unitResolution = {0};
-
-    /** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
-    CV_PROP_RW Mat pose;
-
-    /** @brief Length of voxels in meters */
-    CV_PROP_RW float voxelSize;
-
-    /** @brief TSDF truncation distance
-        Distances greater than value from surface will be truncated to 1.0
-    */
-    CV_PROP_RW float tsdfTruncDist;
-
-    /** @brief Max number of frames to integrate per voxel
-        Represents the max number of frames over which a running average
-        of the TSDF is calculated for a voxel
-    */
-    CV_PROP_RW int maxWeight;
-
-    /** @brief Threshold for depth truncation in meters
-        Truncates the depth greater than threshold to 0
-    */
-    CV_PROP_RW float depthTruncThreshold;
-
-    /** @brief Length of single raycast step
-        Describes the percentage of voxel length that is skipped per march
-    */
-    CV_PROP_RW float raycastStepFactor;
-
-    /** @brief Default set of parameters that provide higher quality reconstruction
-        at the cost of slow performance.
-    */
-    CV_WRAP static Ptr<VolumeParams> defaultParams(int _volumeType);
-
-    /** @brief Coarse set of parameters that provides relatively higher performance
-        at the cost of reconstrution quality.
-    */
-    CV_WRAP static Ptr<VolumeParams> coarseParams(int _volumeType);
-};
-
-
-CV_EXPORTS_W Ptr<Volume> makeVolume(const Ptr<VolumeParams>& _volumeParams);
-CV_EXPORTS_W Ptr<Volume> makeVolume(int _volumeType, float _voxelSize, Matx44f _pose,
-                                    float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold,
-                                    int _resolutionX, int _resolutionY, int _resolutionZ);
-
 }  // namespace cv
-
 #endif // include guard

modules/3d/include/opencv2/3d/volume_settings.hpp

@@ -0,0 +1,213 @@
+// 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_VOLUME_SETTINGS_HPP
+#define OPENCV_3D_VOLUME_SETTINGS_HPP
+
+#include <opencv2/core.hpp>
+#include <opencv2/3d/volume.hpp>
+
+namespace cv
+{
+
+enum class VolumeType
+{
+    TSDF = 0,
+    HashTSDF = 1,
+    ColorTSDF = 2
+};
+
+
+class CV_EXPORTS_W VolumeSettings
+{
+public:
+    /** @brief Constructor of settings for common TSDF volume type.
+    */
+    VolumeSettings();
+
+    /** @brief Constructor of settings for custom Volume type.
+    * @param volumeType volume type.
+    */
+    VolumeSettings(VolumeType volumeType);
+    ~VolumeSettings();
+
+    /** @brief Sets the width of the image for integration.
+    * @param val input value.
+    */
+    void  setIntegrateWidth(int val);
+
+    /** @brief Returns the width of the image for integration.
+    */
+    int   getIntegrateWidth() const;
+
+    /** @brief Sets the height of the image for integration.
+    * @param val input value.
+    */
+    void  setIntegrateHeight(int val);
+
+    /** @brief Returns the height of the image for integration.
+    */
+    int   getIntegrateHeight() const;
+
+
+    /** @brief Sets the width of the raycasted image.
+    * @param val input value.
+    */
+    void  setRaycastWidth(int val);
+
+    /** @brief Returns the width of the raycasted image.
+    */
+    int   getRaycastWidth() const;
+
+    /** @brief Sets the height of the raycasted image.
+    * @param val input value.
+    */
+    void  setRaycastHeight(int val);
+
+    /** @brief Returns the height of the raycasted image.
+    */
+    int   getRaycastHeight() const;
+
+
+    /** @brief Sets depth factor, witch is the number for depth scaling.
+    * @param val input value.
+    */
+    void  setDepthFactor(float val);
+
+    /** @brief Returns depth factor, witch is the number for depth scaling.
+    */
+    float getDepthFactor() const;
+
+    /** @brief Sets the size of voxel.
+    * @param val input value.
+    */
+    void  setVoxelSize(float val);
+
+    /** @brief Returns the size of voxel.
+    */
+    float getVoxelSize() const;
+
+
+    /** @brief Sets TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
+    * @param val input value.
+    */
+    void  setTsdfTruncateDistance(float val);
+
+    /** @brief Returns TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
+    */
+    float getTsdfTruncateDistance() const;
+
+    /** @brief Sets threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
+    * @param val input value.
+    */
+    void  setMaxDepth(float val);
+
+    /** @brief Returns threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
+    */
+    float getMaxDepth() const;
+
+    /** @brief Sets max number of frames to integrate per voxel.
+        Represents the max number of frames over which a running average of the TSDF is calculated for a voxel.
+    * @param val input value.
+    */
+    void  setMaxWeight(int val);
+
+    /** @brief Returns max number of frames to integrate per voxel.
+        Represents the max number of frames over which a running average of the TSDF is calculated for a voxel.
+    */
+    int   getMaxWeight() const;
+
+    /** @brief Sets length of single raycast step.
+        Describes the percentage of voxel length that is skipped per march.
+    * @param val input value.
+    */
+    void  setRaycastStepFactor(float val);
+
+    /** @brief Returns length of single raycast step.
+        Describes the percentage of voxel length that is skipped per march.
+    */
+    float getRaycastStepFactor() const;
+
+    /** @brief Sets volume pose.
+    * @param val input value.
+    */
+    void setVolumePose(InputArray val);
+
+    /** @brief Sets volume pose.
+    * @param val output value.
+    */
+    void getVolumePose(OutputArray val) const;
+
+    /** @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.
+    * @param val input value.
+    */
+    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.
+    * @param val output value.
+    */
+    void getVolumeResolution(OutputArray val) const;
+
+    /** @brief Returns 3 integers representing strides by x, y and z dimension.
+        Can be used to iterate over volume unit raw data.
+    * @param val output value.
+    */
+    void getVolumeDimensions(OutputArray val) const;
+
+    /** @brief Sets intrinsics of camera for integrations.
+    * Format of input:
+    * [ fx  0 cx ]
+    * [  0 fy cy ]
+    * [  0  0  1 ]
+    * where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
+    * @param val input value.
+    */
+    void setCameraIntegrateIntrinsics(InputArray val);
+
+    /** @brief Returns intrinsics of camera for integrations.
+    * Format of output:
+    * [ fx  0 cx ]
+    * [  0 fy cy ]
+    * [  0  0  1 ]
+    * where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
+    * @param val output value.
+    */
+    void getCameraIntegrateIntrinsics(OutputArray val) const;
+
+    /** @brief Sets intrinsics of camera for raycast image.
+    * Format of input:
+    * [ fx  0 cx ]
+    * [  0 fy cy ]
+    * [  0  0  1 ]
+    * where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
+    * @param val input value.
+    */
+    void setCameraRaycastIntrinsics(InputArray val);
+
+    /** @brief Returns intrinsics of camera for raycast image.
+    * Format of output:
+    * [ fx  0 cx ]
+    * [  0 fy cy ]
+    * [  0  0  1 ]
+    * where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
+    * @param val output value.
+    */
+    void getCameraRaycastIntrinsics(OutputArray val) const;
+
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
+};
+
+}
+
+#endif // !OPENCV_3D_VOLUME_SETTINGS_HPP