Merge pull request #29224 from asmorkalov:as/ptcloud2

Dedicated pointcloud module #29224

OpenCV contrib: https://github.com/opencv/opencv_contrib/pull/4134

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [ ] The PR is proposed to the proper branch
- [ ] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake

modules/ptcloud/CMakeLists.txt

@@ -0,0 +1,14 @@
+set(the_description "High level point cloud and mesh operations")
+
+set(debug_modules "")
+if(DEBUG_opencv_ptcloud)
+  list(APPEND debug_modules opencv_highgui)
+endif()
+ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann ${debug_modules}
+    WRAP java objc python js
+)
+ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES})
+
+if(NOT HAVE_EIGEN)
+  message(STATUS "Geometry: Eigen support is disabled. Eigen is Required for Posegraph optimization")
+endif()

modules/ptcloud/include/opencv2/ptcloud.hpp

@@ -0,0 +1,415 @@
+// 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 POINT_CLOUD_MODULE_HPP
+#define POINT_CLOUD_MODULE_HPP
+
+#include "opencv2/core.hpp"
+#include "opencv2/ptcloud/depth.hpp"
+#include "opencv2/ptcloud/volume.hpp"
+#include "opencv2/ptcloud/odometry.hpp"
+#include "opencv2/ptcloud/odometry_frame.hpp"
+#include "opencv2/ptcloud/odometry_settings.hpp"
+
+/**
+@defgroup ptcloud Point Clound Processing
+*/
+
+//! @addtogroup ptcloud
+//! @{
+
+namespace cv {
+
+/** @brief Loads a point cloud from a file.
+ *
+ * The function loads point cloud from the specified file and returns it.
+ * If the cloud cannot be read, throws an error.
+ * Vertex coordinates, normals and colors are returned as they are saved in the file
+ * even if these arrays have different sizes and their elements do not correspond to each other
+ * (which is typical for OBJ files for example)
+ *
+ * Currently, the following file formats are supported:
+ * -  [Wavefront obj file *.obj](https://en.wikipedia.org/wiki/Wavefront_.obj_file)
+ * -  [Polygon File Format *.ply](https://en.wikipedia.org/wiki/PLY_(file_format))
+ *
+ * @param filename Name of the file
+ * @param vertices vertex coordinates, each value contains 3 floats
+ * @param normals per-vertex normals, each value contains 3 floats
+ * @param rgb per-vertex colors, each value contains 3 floats
+ */
+CV_EXPORTS_W void loadPointCloud(const String &filename, OutputArray vertices, OutputArray normals = noArray(), OutputArray rgb = noArray());
+
+/** @brief Saves a point cloud to a specified file.
+ *
+ * The function saves point cloud to the specified file.
+ * File format is chosen based on the filename extension.
+ *
+ * @param filename Name of the file
+ * @param vertices vertex coordinates, each value contains 3 floats
+ * @param normals per-vertex normals, each value contains 3 floats
+ * @param rgb per-vertex colors, each value contains 3 floats
+ */
+CV_EXPORTS_W void savePointCloud(const String &filename, InputArray vertices, InputArray normals = noArray(), InputArray rgb = noArray());
+
+/** @brief Loads a mesh from a file.
+ *
+ * The function loads mesh from the specified file and returns it.
+ * If the mesh cannot be read, throws an error
+ * Vertex attributes (i.e. space and texture coodinates, normals and colors) are returned in same-sized
+ * arrays with corresponding elements having the same indices.
+ * This means that if a face uses a vertex with a normal or a texture coordinate with different indices
+ * (which is typical for OBJ files for example), this vertex will be duplicated for each face it uses.
+ *
+ * Currently, the following file formats are supported:
+ * -  [Wavefront obj file *.obj](https://en.wikipedia.org/wiki/Wavefront_.obj_file) (ONLY TRIANGULATED FACES)
+ * -  [Polygon File Format *.ply](https://en.wikipedia.org/wiki/PLY_(file_format))
+ * @param filename Name of the file
+ * @param vertices vertex coordinates, each value contains 3 floats
+ * @param indices per-face list of vertices, each value is a vector of ints
+ * @param normals per-vertex normals, each value contains 3 floats
+ * @param colors per-vertex colors, each value contains 3 floats
+ * @param texCoords per-vertex texture coordinates, each value contains 2 or 3 floats
+ */
+CV_EXPORTS_W void loadMesh(const String &filename, OutputArray vertices, OutputArrayOfArrays indices,
+                           OutputArray normals = noArray(), OutputArray colors = noArray(),
+                           OutputArray texCoords = noArray());
+
+/** @brief Saves a mesh to a specified file.
+ *
+ * The function saves mesh to the specified file.
+ * File format is chosen based on the filename extension.
+ *
+ * @param filename Name of the file.
+ * @param vertices vertex coordinates, each value contains 3 floats
+ * @param indices per-face list of vertices, each value is a vector of ints
+ * @param normals per-vertex normals, each value contains 3 floats
+ * @param colors per-vertex colors, each value contains 3 floats
+ * @param texCoords per-vertex texture coordinates, each value contains 2 or 3 floats
+ */
+CV_EXPORTS_W void saveMesh(const String &filename, InputArray vertices, InputArrayOfArrays indices,
+                           InputArray normals = noArray(), InputArray colors = noArray(), InputArray texCoords = noArray());
+
+
+//! Triangle fill settings
+enum TriangleShadingType
+{
+    RASTERIZE_SHADING_WHITE  = 0, //!< a white color is used for the whole triangle
+    RASTERIZE_SHADING_FLAT   = 1, //!< a color of 1st vertex of each triangle is used
+    RASTERIZE_SHADING_SHADED = 2  //!< a color is interpolated between 3 vertices with perspective correction
+};
+
+//! Face culling settings: what faces are drawn after face culling
+enum TriangleCullingMode
+{
+    RASTERIZE_CULLING_NONE = 0, //!< all faces are drawn, no culling is actually performed
+    RASTERIZE_CULLING_CW   = 1, //!< triangles which vertices are given in clockwork order are drawn
+    RASTERIZE_CULLING_CCW  = 2  //!< triangles which vertices are given in counterclockwork order are drawn
+};
+
+//! GL compatibility settings
+enum TriangleGlCompatibleMode
+{
+    RASTERIZE_COMPAT_DISABLED = 0, //!< Color and depth have their natural values and converted to internal formats if needed
+    RASTERIZE_COMPAT_INVDEPTH = 1  //!< Color is natural, Depth is transformed from [-zNear; -zFar] to [0; 1]
+    //!< by the following formula: \f$ \frac{z_{far} \left(z + z_{near}\right)}{z \left(z_{far} - z_{near}\right)} \f$ \n
+    //!< In this mode the input/output depthBuf is considered to be in this format,
+    //!< therefore it's faster since there're no conversions performed
+};
+
+/**
+ * @brief Structure to keep settings for rasterization
+ */
+struct CV_EXPORTS_W_SIMPLE TriangleRasterizeSettings
+{
+    TriangleRasterizeSettings();
+
+    CV_WRAP TriangleRasterizeSettings& setShadingType(TriangleShadingType st) { shadingType = st; return *this; }
+    CV_WRAP TriangleRasterizeSettings& setCullingMode(TriangleCullingMode cm) { cullingMode = cm; return *this; }
+    CV_WRAP TriangleRasterizeSettings& setGlCompatibleMode(TriangleGlCompatibleMode gm) { glCompatibleMode = gm; return *this; }
+
+    TriangleShadingType shadingType;
+    TriangleCullingMode cullingMode;
+    TriangleGlCompatibleMode glCompatibleMode;
+};
+
+
+/** @brief Renders a set of triangles on a depth and color image
+ *
+ * Triangles can be drawn white (1.0, 1.0, 1.0), flat-shaded or with a color interpolation between vertices.
+ * In flat-shaded mode the 1st vertex color of each triangle is used to fill the whole triangle.
+ *
+ * The world2cam is an inverted camera pose matrix in fact. It transforms vertices from world to
+ * camera coordinate system.
+ *
+ * The camera coordinate system emulates the OpenGL's coordinate system having coordinate origin in a screen center,
+ * X axis pointing right, Y axis pointing up and Z axis pointing towards the viewer
+ * except that image is vertically flipped after the render.
+ * This means that all visible objects are placed in z-negative area, or exactly in -zNear > z > -zFar since
+ * zNear and zFar are positive.
+ * For example, at fovY = PI/2 the point (0, 1, -1) will be projected to (width/2, 0) screen point,
+ * (1, 0, -1) to (width/2 + height/2, height/2). Increasing fovY makes projection smaller and vice versa.
+ *
+ * The function does not create or clear output images before the rendering. This means that it can be used
+ * for drawing over an existing image or for rendering a model into a 3D scene using pre-filled Z-buffer.
+ *
+ * Empty scene results in a depth buffer filled by the maximum value since every pixel is infinitely far from the camera.
+ * Therefore, before rendering anything from scratch the depthBuf should be filled by zFar values (or by ones in INVDEPTH mode).
+ *
+ * There are special versions of this function named triangleRasterizeDepth and triangleRasterizeColor
+ * for cases if a user needs a color image or a depth image alone; they may run slightly faster.
+ *
+ * @param vertices vertices coordinates array. Should contain values of CV_32FC3 type or a compatible one (e.g. cv::Vec3f, etc.)
+ * @param indices triangle vertices index array, 3 per triangle. Each index indicates a vertex in a vertices array.
+ * Should contain CV_32SC3 values or compatible
+ * @param colors per-vertex colors of CV_32FC3 type or compatible. Can be empty or the same size as vertices array.
+ * If the values are out of [0; 1] range, the result correctness is not guaranteed
+ * @param colorBuf an array representing the final rendered image. Should containt CV_32FC3 values and be the same size as depthBuf.
+ * Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
+ * @param depthBuf an array of floats containing resulting Z buffer. Should contain float values and be the same size as colorBuf.
+ * Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
+ * Empty scene corresponds to all values set to zFar (or to 1.0 in INVDEPTH mode)
+ * @param world2cam a 4x3 or 4x4 float or double matrix containing inverted (sic!) camera pose
+ * @param fovY field of view in vertical direction, given in radians
+ * @param zNear minimum Z value to render, everything closer is clipped
+ * @param zFar maximum Z value to render, everything farther is clipped
+ * @param settings see TriangleRasterizeSettings. By default the smooth shading is on,
+ * with CW culling and with disabled GL compatibility
+ */
+CV_EXPORTS_W void triangleRasterize(InputArray vertices, InputArray indices, InputArray colors,
+                                    InputOutputArray colorBuf, InputOutputArray depthBuf,
+                                    InputArray world2cam, double fovY, double zNear, double zFar,
+                                    const TriangleRasterizeSettings& settings = TriangleRasterizeSettings());
+
+/** @brief Overloaded version of triangleRasterize() with depth-only rendering
+ *
+ * @param vertices vertices coordinates array. Should contain values of CV_32FC3 type or a compatible one (e.g. cv::Vec3f, etc.)
+ * @param indices triangle vertices index array, 3 per triangle. Each index indicates a vertex in a vertices array.
+ * Should contain CV_32SC3 values or compatible
+ * @param depthBuf an array of floats containing resulting Z buffer. Should contain float values and be the same size as colorBuf.
+ * Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
+ * Empty scene corresponds to all values set to zFar (or to 1.0 in INVDEPTH mode)
+ * @param world2cam a 4x3 or 4x4 float or double matrix containing inverted (sic!) camera pose
+ * @param fovY field of view in vertical direction, given in radians
+ * @param zNear minimum Z value to render, everything closer is clipped
+ * @param zFar maximum Z value to render, everything farther is clipped
+ * @param settings see TriangleRasterizeSettings. By default the smooth shading is on,
+ * with CW culling and with disabled GL compatibility
+ */
+CV_EXPORTS_W void triangleRasterizeDepth(InputArray vertices, InputArray indices, InputOutputArray depthBuf,
+                                         InputArray world2cam, double fovY, double zNear, double zFar,
+                                         const TriangleRasterizeSettings& settings = TriangleRasterizeSettings());
+
+/** @brief Overloaded version of triangleRasterize() with color-only rendering
+ *
+ * @param vertices vertices coordinates array. Should contain values of CV_32FC3 type or a compatible one (e.g. cv::Vec3f, etc.)
+ * @param indices triangle vertices index array, 3 per triangle. Each index indicates a vertex in a vertices array.
+ * Should contain CV_32SC3 values or compatible
+ * @param colors per-vertex colors of CV_32FC3 type or compatible. Can be empty or the same size as vertices array.
+ * If the values are out of [0; 1] range, the result correctness is not guaranteed
+ * @param colorBuf an array representing the final rendered image. Should containt CV_32FC3 values and be the same size as depthBuf.
+ * Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
+ * @param world2cam a 4x3 or 4x4 float or double matrix containing inverted (sic!) camera pose
+ * @param fovY field of view in vertical direction, given in radians
+ * @param zNear minimum Z value to render, everything closer is clipped
+ * @param zFar maximum Z value to render, everything farther is clipped
+ * @param settings see TriangleRasterizeSettings. By default the smooth shading is on,
+ * with CW culling and with disabled GL compatibility
+ */
+CV_EXPORTS_W void triangleRasterizeColor(InputArray vertices, InputArray indices, InputArray colors, InputOutputArray colorBuf,
+                                         InputArray world2cam, double fovY, double zNear, double zFar,
+                                         const TriangleRasterizeSettings& settings = TriangleRasterizeSettings());
+
+/** @brief Octree for 3D vision.
+ *
+ * In 3D vision filed, the Octree is used to process and accelerate the pointcloud data. The class Octree represents
+ * the Octree data structure. Each Octree will have a fixed depth. The depth of Octree refers to the distance from
+ * the root node to the leaf node.All OctreeNodes will not exceed this depth.Increasing the depth will increase
+ * the amount of calculation exponentially. And the small number of depth refers low resolution of Octree.
+ * Each node contains 8 children, which are used to divide the space cube into eight parts. Each octree node represents
+ * a cube. And these eight children will have a fixed order, the order is described as follows:
+ *
+ * For illustration, assume,
+ *
+ * rootNode: origin == (0, 0, 0), size == 2
+ *
+ * Then,
+ *
+ * children[0]: origin == (0, 0, 0), size == 1
+ *
+ * children[1]: origin == (1, 0, 0), size == 1, along X-axis next to child 0
+ *
+ * children[2]: origin == (0, 1, 0), size == 1, along Y-axis next to child 0
+ *
+ * children[3]: origin == (1, 1, 0), size == 1, in X-Y plane
+ *
+ * children[4]: origin == (0, 0, 1), size == 1, along Z-axis next to child 0
+ *
+ * children[5]: origin == (1, 0, 1), size == 1, in X-Z plane
+ *
+ * children[6]: origin == (0, 1, 1), size == 1, in Y-Z plane
+ *
+ * children[7]: origin == (1, 1, 1), size == 1, furthest from child 0
+ */
+
+class CV_EXPORTS_W Octree
+{
+public:
+    //! Default constructor.
+    Octree();
+
+    /** @overload
+     * @brief Creates an empty Octree with given maximum depth
+     *
+     * @param maxDepth The max depth of the Octree
+     * @param size bounding box size for the Octree
+     * @param origin Initial center coordinate
+     * @param withColors Whether to keep per-point colors or not
+     * @return resulting Octree
+     */
+    CV_WRAP static Ptr<Octree> createWithDepth(int maxDepth, double size, const Point3f& origin = { }, bool withColors = false);
+
+    /** @overload
+     * @brief Create an Octree from the PointCloud data with the specific maxDepth
+     *
+     * @param maxDepth Max depth of the octree
+     * @param pointCloud point cloud data, should be 3-channel float array
+     * @param colors color attribute of point cloud in the same 3-channel float format
+     * @return resulting Octree
+     */
+    CV_WRAP static Ptr<Octree> createWithDepth(int maxDepth, InputArray pointCloud, InputArray colors = noArray());
+
+    /** @overload
+     * @brief Creates an empty Octree with given resolution
+     *
+     * @param resolution The size of the octree leaf node
+     * @param size bounding box size for the Octree
+     * @param origin Initial center coordinate
+     * @param withColors Whether to keep per-point colors or not
+     * @return resulting Octree
+     */
+    CV_WRAP static Ptr<Octree> createWithResolution(double resolution, double size, const Point3f& origin = { }, bool withColors = false);
+
+    /** @overload
+     * @brief Create an Octree from the PointCloud data with the specific resolution
+     *
+     * @param resolution The size of the octree leaf node
+     * @param pointCloud point cloud data, should be 3-channel float array
+     * @param colors color attribute of point cloud in the same 3-channel float format
+     * @return resulting octree
+     */
+    CV_WRAP static Ptr<Octree> createWithResolution(double resolution, InputArray pointCloud, InputArray colors = noArray());
+
+    //! Default destructor
+    ~Octree();
+
+    /** @overload
+     * @brief Insert a point data with color to a OctreeNode.
+     *
+     * @param point The point data in Point3f format.
+     * @param color The color attribute of point in Point3f format.
+     * @return Returns whether the insertion is successful.
+     */
+    CV_WRAP bool insertPoint(const Point3f& point, const Point3f& color = { });
+
+    /** @brief Determine whether the point is within the space range of the specific cube.
+     *
+     * @param point The point coordinates.
+     * @return If point is in bound, return ture. Otherwise, false.
+     */
+    CV_WRAP bool isPointInBound(const Point3f& point) const;
+
+    //! returns true if the rootnode is NULL.
+    CV_WRAP bool empty() const;
+
+    /** @brief Reset all octree parameter.
+     *
+     *  Clear all the nodes of the octree and initialize the parameters.
+     */
+    CV_WRAP void clear();
+
+    /** @brief Delete a given point from the Octree.
+     *
+     * Delete the corresponding element from the pointList in the corresponding leaf node. If the leaf node
+     * does not contain other points after deletion, this node will be deleted. In the same way,
+     * its parent node may also be deleted if its last child is deleted.
+     * @param point The point coordinates, comparison is epsilon-based
+     * @return return ture if the point is deleted successfully.
+     */
+    CV_WRAP bool deletePoint(const Point3f& point);
+
+    /** @brief restore point cloud data from Octree.
+     *
+     * Restore the point cloud data from existing octree. The points in same leaf node will be seen as the same point.
+     * This point is the center of the leaf node. If the resolution is small, it will work as a downSampling function.
+     * @param restoredPointCloud The output point cloud data, can be replaced by noArray() if not needed
+     * @param restoredColor The color attribute of point cloud data, can be omitted if not needed
+     */
+    CV_WRAP void getPointCloudByOctree(OutputArray restoredPointCloud, OutputArray restoredColor = noArray());
+
+    /** @brief Radius Nearest Neighbor Search in Octree.
+     *
+     * Search all points that are less than or equal to radius.
+     * And return the number of searched points.
+     * @param query Query point.
+     * @param radius Retrieved radius value.
+     * @param points Point output. Contains searched points in 3-float format, and output vector is not in order,
+     * can be replaced by noArray() if not needed
+     * @param squareDists Dist output. Contains searched squared distance in floats, and output vector is not in order,
+     * can be omitted if not needed
+     * @return the number of searched points.
+     */
+    CV_WRAP int radiusNNSearch(const Point3f& query, float radius, OutputArray points, OutputArray squareDists = noArray()) const;
+
+    /** @overload
+     *  @brief Radius Nearest Neighbor Search in Octree.
+     *
+     * Search all points that are less than or equal to radius.
+     * And return the number of searched points.
+     * @param query Query point.
+     * @param radius Retrieved radius value.
+     * @param points Point output. Contains searched points in 3-float format, and output vector is not in order,
+     * can be replaced by noArray() if not needed
+     * @param colors Color output. Contains colors corresponding to points in pointSet, can be replaced by noArray() if not needed
+     * @param squareDists Dist output. Contains searched squared distance in floats, and output vector is not in order,
+     * can be replaced by noArray() if not needed
+     * @return the number of searched points.
+     */
+    CV_WRAP int radiusNNSearch(const Point3f& query, float radius, OutputArray points, OutputArray colors, OutputArray squareDists) const;
+
+    /** @brief K Nearest Neighbor Search in Octree.
+     *
+     * Find the K nearest neighbors to the query point.
+     * @param query Query point.
+     * @param K amount of nearest neighbors to find
+     * @param points Point output. Contains K points in 3-float format, arranged in order of distance from near to far,
+     * can be replaced by noArray() if not needed
+     * @param squareDists Dist output. Contains K squared distance in floats, arranged in order of distance from near to far,
+     * can be omitted if not needed
+     */
+    CV_WRAP void KNNSearch(const Point3f& query, const int K, OutputArray points, OutputArray squareDists = noArray()) const;
+
+    /** @overload
+     *  @brief K Nearest Neighbor Search in Octree.
+     *
+     * Find the K nearest neighbors to the query point.
+     * @param query Query point.
+     * @param K amount of nearest neighbors to find
+     * @param points Point output. Contains K points in 3-float format, arranged in order of distance from near to far,
+     * can be replaced by noArray() if not needed
+     * @param colors Color output. Contains colors corresponding to points in pointSet, can be replaced by noArray() if not needed
+     * @param squareDists Dist output. Contains K squared distance in floats, arranged in order of distance from near to far,
+     * can be replaced by noArray() if not needed
+     */
+    CV_WRAP void KNNSearch(const Point3f& query, const int K, OutputArray points, OutputArray colors, OutputArray squareDists) const;
+
+protected:
+    struct Impl;
+    Ptr<Impl> p;
+};
+
+} // namespace cv
+
+//! @} ptcloud
+
+#endif

modules/ptcloud/include/opencv2/ptcloud/depth.hpp

@@ -0,0 +1,181 @@
+// 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_DEPTH_HPP
+#define OPENCV_3D_DEPTH_HPP
+
+#include <opencv2/core.hpp>
+#include <limits>
+
+namespace cv
+{
+//! @addtogroup rgbd
+//! @{
+
+/** Object that can compute the normals in an image.
+ * It is an object as it can cache data for speed efficiency
+ * The implemented methods are either:
+ * - FALS (the fastest) and SRI from
+ * ``Fast and Accurate Computation of Surface Normals from Range Images``
+ * by H. Badino, D. Huber, Y. Park and T. Kanade
+ * - the normals with bilateral filtering on a depth image from
+ * ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
+ * by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
+ */
+class CV_EXPORTS_W RgbdNormals
+{
+public:
+    enum RgbdNormalsMethod
+    {
+      RGBD_NORMALS_METHOD_FALS = 0,
+      RGBD_NORMALS_METHOD_LINEMOD = 1,
+      RGBD_NORMALS_METHOD_SRI = 2,
+      RGBD_NORMALS_METHOD_CROSS_PRODUCT = 3
+    };
+
+    RgbdNormals() { }
+    virtual ~RgbdNormals() { }
+
+    /** Creates new RgbdNormals object
+     * @param rows the number of rows of the depth image normals will be computed on
+     * @param cols the number of cols of the depth image normals will be computed on
+     * @param depth the depth of the normals (only CV_32F or CV_64F)
+     * @param K the calibration matrix to use
+     * @param window_size the window size to compute the normals: can only be 1,3,5 or 7
+     * @param diff_threshold threshold in depth difference, used in LINEMOD algirithm
+     * @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS
+     */
+    CV_WRAP static Ptr<RgbdNormals> create(int rows = 0, int cols = 0, int depth = 0, InputArray K = noArray(), int window_size = 5,
+                                           float diff_threshold = 50.f,
+                                           RgbdNormals::RgbdNormalsMethod method = RgbdNormals::RgbdNormalsMethod::RGBD_NORMALS_METHOD_FALS);
+
+    /** Given a set of 3d points in a depth image, compute the normals at each point.
+     * @param points a rows x cols x 3 matrix of CV_32F/CV64F or a rows x cols x 1 CV_U16S
+     * @param normals a rows x cols x 3 matrix
+     */
+    CV_WRAP virtual void apply(InputArray points, OutputArray normals) const = 0;
+
+    /** Prepares cached data required for calculation
+    * If not called by user, called automatically at first calculation
+    */
+    CV_WRAP virtual void cache() const = 0;
+
+    CV_WRAP virtual int getRows() const = 0;
+    CV_WRAP virtual void setRows(int val) = 0;
+    CV_WRAP virtual int getCols() const = 0;
+    CV_WRAP virtual void setCols(int val) = 0;
+    CV_WRAP virtual int getWindowSize() const = 0;
+    CV_WRAP virtual void setWindowSize(int val) = 0;
+    CV_WRAP virtual int getDepth() const = 0;
+    CV_WRAP virtual void getK(OutputArray val) const = 0;
+    CV_WRAP virtual void setK(InputArray val) = 0;
+    CV_WRAP virtual RgbdNormals::RgbdNormalsMethod getMethod() const = 0;
+};
+
+
+/** Registers depth data to an external camera
+ * Registration is performed by creating a depth cloud, transforming the cloud by
+ * the rigid body transformation between the cameras, and then projecting the
+ * transformed points into the RGB camera.
+ *
+ * uv_rgb = K_rgb * [R | t] * z * inv(K_ir) * uv_ir
+ *
+ * Currently does not check for negative depth values.
+ *
+ * @param unregisteredCameraMatrix the camera matrix of the depth camera
+ * @param registeredCameraMatrix the camera matrix of the external camera
+ * @param registeredDistCoeffs the distortion coefficients of the external camera
+ * @param Rt the rigid body transform between the cameras. Transforms points from depth camera frame to external camera frame.
+ * @param unregisteredDepth the input depth data
+ * @param outputImagePlaneSize the image plane dimensions of the external camera (width, height)
+ * @param registeredDepth the result of transforming the depth into the external camera
+ * @param depthDilation whether or not the depth is dilated to avoid holes and occlusion errors (optional)
+ */
+CV_EXPORTS_W void registerDepth(InputArray unregisteredCameraMatrix, InputArray registeredCameraMatrix, InputArray registeredDistCoeffs,
+                                InputArray Rt, InputArray unregisteredDepth, const Size& outputImagePlaneSize,
+                                OutputArray registeredDepth, bool depthDilation=false);
+
+/**
+ * @param depth the depth image
+ * @param in_K
+ * @param in_points the list of xy coordinates
+ * @param points3d the resulting 3d points (point is represented by 4 chanels value [x, y, z, 0])
+ */
+CV_EXPORTS_W void depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d);
+
+/** Converts a depth image to 3d points. If the mask is empty then the resulting array has the same dimensions as `depth`,
+ * otherwise it is 1d vector containing mask-enabled values only.
+ * The coordinate system is x pointing left, y down and z away from the camera
+ * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), otherwise, if given as CV_32F or CV_64F, it is assumed in meters)
+ * @param K The calibration matrix
+ * @param points3d the resulting 3d points (point is represented by 4 channels value [x, y, z, 0]). They are of the same depth as `depth` if it is CV_32F or CV_64F, and the
+ *        depth of `K` if `depth` is of depth CV_16U or CV_16S
+ * @param mask the mask of the points to consider (can be empty)
+ */
+CV_EXPORTS_W void depthTo3d(InputArray depth, InputArray K, OutputArray points3d, InputArray mask = noArray());
+
+/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+ * by depth_factor to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+ * Otherwise, the image is simply converted to floats
+ * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), it is assumed in meters)
+ * @param type the desired output depth (CV_32F or CV_64F)
+ * @param out The rescaled float depth image
+ * @param depth_factor (optional) factor by which depth is converted to distance (by default = 1000.0 for Kinect sensor)
+ */
+CV_EXPORTS_W void rescaleDepth(InputArray in, int type, OutputArray out, double depth_factor = 1000.0);
+
+/** Warps depth or RGB-D image by reprojecting it in 3d, applying Rt transformation
+ * and then projecting it back onto the image plane.
+ * This function can be used to visualize the results of the Odometry algorithm.
+ * @param depth Depth data, should be 1-channel CV_16U, CV_16S, CV_32F or CV_64F
+ * @param image RGB image (optional), should be 1-, 3- or 4-channel CV_8U
+ * @param mask Mask of used pixels (optional), should be CV_8UC1, CV_8SC1 or CV_BoolC1
+ * @param Rt Rotation+translation matrix (3x4 or 4x4) to be applied to depth points
+ * @param cameraMatrix Camera intrinsics matrix (3x3)
+ * @param warpedDepth The warped depth data (optional)
+ * @param warpedImage The warped RGB image (optional)
+ * @param warpedMask The mask of valid pixels in warped image (optional)
+ */
+CV_EXPORTS_W void warpFrame(InputArray depth, InputArray image, InputArray mask, InputArray Rt, InputArray cameraMatrix,
+                            OutputArray warpedDepth = noArray(), OutputArray warpedImage = noArray(), OutputArray warpedMask = noArray());
+
+enum RgbdPlaneMethod
+{
+    RGBD_PLANE_METHOD_DEFAULT
+};
+
+/** Find the planes in a depth image
+ * @param points3d the 3d points organized like the depth image: rows x cols with 3 channels
+ * @param normals the normals for every point in the depth image; optional, can be empty
+ * @param mask An image where each pixel is labeled with the plane it belongs to
+ *        and 255 if it does not belong to any plane
+ * @param plane_coefficients the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0, norm(a,b,c)=1
+ *        and c < 0 (so that the normal points towards the camera)
+ * @param block_size The size of the blocks to look at for a stable MSE
+ * @param min_size The minimum size of a cluster to be considered a plane
+ * @param threshold The maximum distance of a point from a plane to belong to it (in meters)
+ * @param sensor_error_a coefficient of the sensor error. 0 by default, use 0.0075 for a Kinect
+ * @param sensor_error_b coefficient of the sensor error. 0 by default
+ * @param sensor_error_c coefficient of the sensor error. 0 by default
+ * @param method The method to use to compute the planes.
+ */
+CV_EXPORTS_W void findPlanes(InputArray points3d, InputArray normals, OutputArray mask, OutputArray plane_coefficients,
+                             int block_size = 40, int min_size = 40*40, double threshold = 0.01,
+                             double sensor_error_a = 0, double sensor_error_b = 0,
+                             double sensor_error_c = 0,
+                             RgbdPlaneMethod method = RGBD_PLANE_METHOD_DEFAULT);
+
+
+
+// TODO Depth interpolation
+// Curvature
+// Get rescaleDepth return dubles if asked for
+
+//! @}
+
+} /* namespace cv */
+
+#endif // include guard

modules/ptcloud/include/opencv2/ptcloud/detail/kinfu_frame.hpp

@@ -0,0 +1,19 @@
+// 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_DETAIL_KINFU_FRAME_HPP
+#define OPENCV_3D_DETAIL_KINFU_FRAME_HPP
+
+#include <opencv2/core/affine.hpp>
+
+namespace cv {
+namespace detail {
+
+CV_EXPORTS void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Vec3f lightLoc);
+CV_EXPORTS void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image);
+
+} // namespace detail
+} // namespace cv
+
+#endif // include guard

modules/ptcloud/include/opencv2/ptcloud/detail/pose_graph.hpp

@@ -0,0 +1,77 @@
+// 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 POINTCLOUD_POSE_GRAPTH_HPP
+#define POINTCLOUD_POSE_GRAPTH_HPP
+
+#include "opencv2/core/affine.hpp"
+#include "opencv2/core/quaternion.hpp"
+#include "opencv2/geometry/3d.hpp"
+#include "opencv2/geometry/detail/optimizer.hpp"
+
+namespace cv
+{
+namespace detail
+{
+
+// ATTENTION! This class is used internally in Large KinFu.
+// It has been pushed to publicly available headers for tests only.
+// Source compatibility of this API is not guaranteed in the future.
+
+// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems
+// The pose graph format, cost function and optimization techniques
+// repeat the ones used in Ceres 3D Pose Graph Optimization:
+// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet
+class CV_EXPORTS_W PoseGraph
+{
+public:
+    static Ptr<PoseGraph> create();
+    virtual ~PoseGraph();
+
+    // Node may have any id >= 0
+    virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0;
+    virtual bool isNodeExist(size_t nodeId) const = 0;
+    virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0;
+    virtual bool isNodeFixed(size_t nodeId) const = 0;
+    virtual Affine3d getNodePose(size_t nodeId) const = 0;
+    virtual std::vector<size_t> getNodesIds() const = 0;
+    virtual size_t getNumNodes() const = 0;
+
+    // Edges have consequent indices starting from 0
+    virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation,
+                         const Matx66f& _information = Matx66f::eye()) = 0;
+    virtual size_t getEdgeStart(size_t i) const = 0;
+    virtual size_t getEdgeEnd(size_t i) const = 0;
+    virtual Affine3d getEdgePose(size_t i) const = 0;
+    virtual Matx66f getEdgeInfo(size_t i) const = 0;
+    virtual size_t getNumEdges() const = 0;
+
+    // checks if graph is connected and each edge connects exactly 2 nodes
+    virtual bool isValid() const = 0;
+
+    // Calculates an initial pose estimate using the Minimum Spanning Tree (Prim's MST) algorithm.
+    // The result serves as a starting point for further optimization.
+    // Edge weights are calculated as:
+    //     weight = translationNorm + lambda * rotationAngle
+    // The default lambda value (0.485) was empirically chosen based on its impact on optimizer performance,
+    // but can/should be tuned for different datasets.
+    virtual void initializePosesWithMST(double lambda = 0.485) = 0;
+
+    // creates an optimizer with user-defined settings and returns a pointer on it
+    virtual Ptr<cv::detail::LevMarqBase> createOptimizer(const LevMarq::Settings& settings) = 0;
+    // creates an optimizer with default settings and returns a pointer on it
+    virtual Ptr<cv::detail::LevMarqBase> createOptimizer() = 0;
+
+    // Creates an optimizer (with default settings) if it wasn't created before and runs it
+    // Returns number of iterations elapsed or -1 if failed to optimize
+    virtual LevMarq::Report optimize() = 0;
+
+    // calculate cost function based on current nodes parameters
+    virtual double calcEnergy() const = 0;
+};
+
+}  // namespace detail
+}  // namespace cv
+
+#endif // include guard

modules/ptcloud/include/opencv2/ptcloud/detail/submap.hpp

@@ -0,0 +1,558 @@
+// 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_DETAIL_SUBMAP_HPP
+#define OPENCV_3D_DETAIL_SUBMAP_HPP
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/affine.hpp>
+#include "opencv2/geometry/detail/optimizer.hpp"
+
+//TODO: remove it when it is rewritten to robust pose graph
+#include "opencv2/core/dualquaternion.hpp"
+
+#include <type_traits>
+#include <vector>
+#include <map>
+#include <unordered_map>
+
+
+namespace cv
+{
+namespace detail
+{
+template<typename MatType>
+class Submap
+{
+public:
+    struct PoseConstraint
+    {
+        Affine3f estimatedPose;
+        int weight;
+
+        PoseConstraint() : weight(0){};
+
+        void accumulatePose(const Affine3f& _pose, int _weight = 1)
+        {
+            DualQuatf accPose = DualQuatf::createFromAffine3(estimatedPose) * float(weight) + DualQuatf::createFromAffine3(_pose) * float(_weight);
+            weight += _weight;
+            accPose = accPose / float(weight);
+            estimatedPose = accPose.toAffine3();
+        }
+    };
+    typedef std::map<int, PoseConstraint> Constraints;
+
+    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),
+          volume(VolumeType::HashTSDF, settings)
+    { }
+    virtual ~Submap() = default;
+
+    virtual void integrate(InputArray _depth, const int currframeId);
+    virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, cv::Matx33f K,
+                         OutputArray points = noArray(), OutputArray normals = noArray());
+
+    virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); };
+    virtual int getVisibleBlocks(int currFrameId) const
+    {
+        CV_Assert(currFrameId >= startFrameId);
+        //return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
+        return volume.getVisibleBlocks();
+
+    }
+
+    float calcVisibilityRatio(int currFrameId) const
+    {
+        int allocate_blocks = getTotalAllocatedBlocks();
+        int visible_blocks  = getVisibleBlocks(currFrameId);
+        return float(visible_blocks) / float(allocate_blocks);
+    }
+
+    // Adding new Edge for Loop Closure Detection. Return true or false to indicate whether adding success.
+    bool addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose);
+
+    //! TODO: Possibly useless
+    virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; };
+    virtual void setStopFrameId(int _stopFrameId) { stopFrameId = _stopFrameId; };
+
+    void composeCameraPose(const cv::Affine3f& _relativePose) { cameraPose = cameraPose * _relativePose; }
+    PoseConstraint& getConstraint(const int _id)
+    {
+        //! Creates constraints if doesn't exist yet
+        return constraints[_id];
+    }
+
+public:
+    const int id;
+    cv::Affine3f pose;
+    cv::Affine3f cameraPose;
+    Constraints constraints;
+
+    int startFrameId;
+    int stopFrameId;
+    //! TODO: Should we support submaps for regular volumes?
+    static constexpr int FRAME_VISIBILITY_THRESHOLD = 5;
+
+    //! TODO: Add support for GPU arrays (UMat)
+    OdometryFrame frame;
+    OdometryFrame renderFrame;
+
+    Volume volume;
+};
+
+template<typename MatType>
+
+void Submap<MatType>::integrate(InputArray _depth, const int currFrameId)
+{
+    CV_Assert(currFrameId >= startFrameId);
+    volume.integrate(_depth, cameraPose.matrix);
+}
+
+template<typename MatType>
+void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, cv::Matx33f K,
+                              OutputArray points, OutputArray normals)
+{
+    if (!points.needed() && !normals.needed())
+    {
+        MatType pts, nrm;
+        //TODO: get depth instead of pts from raycast
+        volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, pts, nrm);
+
+        std::vector<MatType> pch(3);
+        split(pts, pch);
+
+        renderFrame = frame;
+
+        frame = OdometryFrame(pch[2]);
+    }
+    else
+    {
+        volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, points, normals);
+    }
+}
+
+template<typename MatType>
+bool Submap<MatType>::addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose)
+{
+    auto iter = constraints.find(tarSubmapID);
+
+    // if there is NO edge of currSubmap to tarSubmap.
+    if(iter == constraints.end())
+    {
+        // Frome pose to tarPose transformation
+        Affine3f estimatePose = tarPose * pose.inv();
+
+        // Create new Edge.
+        PoseConstraint& preConstrain = getConstraint(tarSubmapID);
+        preConstrain.accumulatePose(estimatePose, 1);
+
+        return true;
+    } else
+    {
+        return false;
+    }
+}
+
+/**
+ * @brief: Manages all the created submaps for a particular scene
+ */
+template<typename MatType>
+class SubmapManager
+{
+public:
+    enum class Type
+    {
+        NEW            = 0,
+        CURRENT        = 1,
+        RELOCALISATION = 2,
+        LOOP_CLOSURE   = 3,
+        LOST           = 4
+    };
+
+    struct ActiveSubmapData
+    {
+        Type type;
+        std::vector<Affine3f> constraints;
+        int trackingAttempts;
+    };
+
+    typedef Submap<MatType> SubmapT;
+    typedef std::map<int, Ptr<SubmapT>> IdToSubmapPtr;
+    typedef std::unordered_map<int, ActiveSubmapData> IdToActiveSubmaps;
+
+    explicit SubmapManager(const VolumeSettings& _volumeSettings) : volumeSettings(_volumeSettings) {}
+    virtual ~SubmapManager() = default;
+
+    void reset() { submapList.clear(); };
+
+    bool shouldCreateSubmap(int frameId);
+    bool shouldChangeCurrSubmap(int _frameId, int toSubmapId);
+
+    //! Adds a new submap/volume into the current list of managed/Active submaps
+    int createNewSubmap(bool isCurrentActiveMap, const int currFrameId = 0, const Affine3f& pose = cv::Affine3f::Identity());
+
+    void removeSubmap(int _id);
+    size_t numOfSubmaps(void) const { return submapList.size(); };
+    size_t numOfActiveSubmaps(void) const { return activeSubmaps.size(); };
+
+    Ptr<SubmapT> getSubmap(int _id) const;
+    Ptr<SubmapT> getCurrentSubmap(void) const;
+
+    int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose);
+    bool updateMap(int frameId, const OdometryFrame& frame);
+
+    bool addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID);
+
+    Ptr<detail::PoseGraph> MapToPoseGraph();
+    void PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph);
+
+    VolumeSettings volumeSettings;
+
+    std::vector<Ptr<SubmapT>> submapList;
+    IdToActiveSubmaps activeSubmaps;
+
+    Ptr<detail::PoseGraph> poseGraph;
+};
+
+template<typename MatType>
+int SubmapManager<MatType>::createNewSubmap(bool isCurrentMap, int currFrameId, const Affine3f& pose)
+{
+    int newId = int(submapList.size());
+
+    Ptr<SubmapT> newSubmap = cv::makePtr<SubmapT>(newId, volumeSettings, pose, currFrameId);
+    submapList.push_back(newSubmap);
+
+    ActiveSubmapData newSubmapData;
+    newSubmapData.trackingAttempts = 0;
+    newSubmapData.type             = isCurrentMap ? Type::CURRENT : Type::NEW;
+    activeSubmaps[newId]           = newSubmapData;
+
+    return newId;
+}
+
+template<typename MatType>
+Ptr<Submap<MatType>> SubmapManager<MatType>::getSubmap(int _id) const
+{
+    CV_Assert(submapList.size() > 0);
+    CV_Assert(_id >= 0 && _id < int(submapList.size()));
+    return submapList.at(_id);
+}
+
+template<typename MatType>
+Ptr<Submap<MatType>> SubmapManager<MatType>::getCurrentSubmap(void) const
+{
+    for (const auto& it : activeSubmaps)
+    {
+        if (it.second.type == Type::CURRENT)
+            return getSubmap(it.first);
+    }
+    return nullptr;
+}
+
+template<typename MatType>
+bool SubmapManager<MatType>::shouldCreateSubmap(int currFrameId)
+{
+    int currSubmapId = -1;
+    for (const auto& it : activeSubmaps)
+    {
+        auto submapData = it.second;
+        // No more than 1 new submap at a time!
+        if (submapData.type == Type::NEW)
+        {
+            return false;
+        }
+        if (submapData.type == Type::CURRENT)
+        {
+            currSubmapId = it.first;
+        }
+    }
+    //! TODO: This shouldn't be happening? since there should always be one active current submap
+    if (currSubmapId < 0)
+    {
+        return false;
+    }
+
+    Ptr<SubmapT> currSubmap = getSubmap(currSubmapId);
+    float ratio             = currSubmap->calcVisibilityRatio(currFrameId);
+
+    //TODO: fix this when a new pose graph is ready
+    // if (ratio < 0.2f)
+    if (ratio < 0.5f)
+        return true;
+    return false;
+}
+
+template<typename MatType>
+int SubmapManager<MatType>::estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose)
+{
+    static constexpr int MAX_ITER                    = 10;
+    static constexpr float CONVERGE_WEIGHT_THRESHOLD = 0.01f;
+    static constexpr float INLIER_WEIGHT_THRESH      = 0.8f;
+    static constexpr int MIN_INLIERS                 = 10;
+    static constexpr int MAX_TRACKING_ATTEMPTS = 25;
+
+    //! thresh = HUBER_THRESH
+    auto huberWeight = [](float residual, float thresh = 0.1f) -> float {
+        float rAbs = abs(residual);
+        if (rAbs < thresh)
+            return 1.0;
+        float numerator = sqrt(2 * thresh * rAbs - thresh * thresh);
+        return numerator / rAbs;
+    };
+
+    Ptr<SubmapT> fromSubmap          = getSubmap(fromSubmapId);
+    Ptr<SubmapT> toSubmap            = getSubmap(toSubmapId);
+    ActiveSubmapData& fromSubmapData = activeSubmaps.at(fromSubmapId);
+
+    Affine3f TcameraToFromSubmap = fromSubmap->cameraPose;
+    Affine3f TcameraToToSubmap   = toSubmap->cameraPose;
+
+    // FromSubmap -> ToSubmap transform
+    Affine3f candidateConstraint = TcameraToToSubmap * TcameraToFromSubmap.inv();
+    fromSubmapData.trackingAttempts++;
+    fromSubmapData.constraints.push_back(candidateConstraint);
+
+    std::vector<float> weights(fromSubmapData.constraints.size() + 1, 1.0f);
+
+    Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose;
+    int prevWeight          = fromSubmap->getConstraint(toSubmap->id).weight;
+
+    // Iterative reweighted least squares with huber threshold to find the inliers in the past observations
+    Vec6f meanConstraint;
+    float sumWeight = 0.0f;
+    for (int i = 0; i < MAX_ITER; i++)
+    {
+        Vec6f constraintVec;
+        for (int j = 0; j < int(weights.size() - 1); j++)
+        {
+            Affine3f currObservation = fromSubmapData.constraints[j];
+            cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec);
+            meanConstraint += weights[j] * constraintVec;
+            sumWeight += weights[j];
+        }
+        // Heavier weight given to the estimatedPose
+        cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec);
+        meanConstraint += weights.back() * prevWeight * constraintVec;
+        sumWeight += prevWeight;
+        meanConstraint /= float(sumWeight);
+
+        float residual = 0.0f;
+        float diff     = 0.0f;
+        for (int j = 0; j < int(weights.size()); j++)
+        {
+            int w;
+            if (j == int(weights.size() - 1))
+            {
+                cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec);
+                w = prevWeight;
+            }
+            else
+            {
+                Affine3f currObservation = fromSubmapData.constraints[j];
+                cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec);
+                w = 1;
+            }
+
+            cv::Vec6f residualVec = (constraintVec - meanConstraint);
+            residual         = float(norm(residualVec));
+            float newWeight = huberWeight(residual);
+            diff += w * abs(newWeight - weights[j]);
+            weights[j] = newWeight;
+        }
+
+        if (diff / (prevWeight + weights.size() - 1) < CONVERGE_WEIGHT_THRESHOLD)
+            break;
+    }
+
+    int localInliers = 0;
+    DualQuatf inlierConstraint;
+    for (int i = 0; i < int(weights.size()); i++)
+    {
+        if (weights[i] > INLIER_WEIGHT_THRESH)
+        {
+            localInliers++;
+            if (i == int(weights.size() - 1))
+                inlierConstraint += DualQuatf::createFromMat(prevConstraint.matrix);
+            else
+                inlierConstraint += DualQuatf::createFromMat(fromSubmapData.constraints[i].matrix);
+        }
+    }
+    inlierConstraint = inlierConstraint * 1.0f/float(max(localInliers, 1));
+    inlierPose = inlierConstraint.toAffine3();
+    inliers    = localInliers;
+
+    if (inliers >= MIN_INLIERS)
+    {
+        return 1;
+    }
+    if(fromSubmapData.trackingAttempts - inliers > (MAX_TRACKING_ATTEMPTS - MIN_INLIERS))
+    {
+        return -1;
+    }
+
+    return 0;
+}
+
+template<typename MatType>
+bool SubmapManager<MatType>::shouldChangeCurrSubmap(int _frameId, int toSubmapId)
+{
+    auto toSubmap         = getSubmap(toSubmapId);
+    auto toSubmapData     = activeSubmaps.at(toSubmapId);
+    auto currActiveSubmap = getCurrentSubmap();
+
+    int blocksInNewMap = toSubmap->getTotalAllocatedBlocks();
+    float newRatio     = toSubmap->calcVisibilityRatio(_frameId);
+
+    float currRatio = currActiveSubmap->calcVisibilityRatio(_frameId);
+
+    //! TODO: Check for a specific threshold?
+    if (blocksInNewMap <= 0)
+        return false;
+    if ((newRatio > currRatio) && (toSubmapData.type == Type::NEW))
+        return true;
+
+    return false;
+}
+
+template<typename MatType>
+bool SubmapManager<MatType>::addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID)
+{
+    Ptr<SubmapT> currentSubmap = getSubmap(currentSubmapID);
+    Ptr<SubmapT> tarSubmap = getSubmap(tarSubmapID);
+
+    return currentSubmap->addEdgeToSubmap(tarSubmapID, tarSubmap->pose);
+}
+
+template<typename MatType>
+bool SubmapManager<MatType>::updateMap(int _frameId, const OdometryFrame& _frame)
+{
+    bool mapUpdated = false;
+    int changedCurrentMapId = -1;
+
+    const int currSubmapId  = getCurrentSubmap()->id;
+
+    for (auto& it : activeSubmaps)
+    {
+        int submapId     = it.first;
+        auto& submapData = it.second;
+        if (submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE)
+        {
+            // Check with previous estimate
+            int inliers;
+            Affine3f inlierPose;
+            int constraintUpdate = estimateConstraint(submapId, currSubmapId, inliers, inlierPose);
+            if (constraintUpdate == 1)
+            {
+                typename SubmapT::PoseConstraint& submapConstraint = getSubmap(submapId)->getConstraint(currSubmapId);
+                submapConstraint.accumulatePose(inlierPose, inliers);
+                submapData.constraints.clear();
+                submapData.trackingAttempts = 0;
+
+                if (shouldChangeCurrSubmap(_frameId, submapId))
+                {
+                    changedCurrentMapId = submapId;
+                }
+                mapUpdated = true;
+            }
+            else if(constraintUpdate == -1)
+            {
+                submapData.type = Type::LOST;
+            }
+        }
+    }
+
+    std::vector<int> createNewConstraintsList;
+    for (auto& it : activeSubmaps)
+    {
+        int submapId     = it.first;
+        auto& submapData = it.second;
+
+        if (submapId == changedCurrentMapId)
+        {
+            submapData.type = Type::CURRENT;
+        }
+        if ((submapData.type == Type::CURRENT) && (changedCurrentMapId >= 0) && (submapId != changedCurrentMapId))
+        {
+            submapData.type = Type::LOST;
+            createNewConstraintsList.push_back(submapId);
+        }
+        if ((submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) && (changedCurrentMapId >= 0))
+        {
+            //! TODO: Add a new type called NEW_LOST?
+            submapData.type = Type::LOST;
+            createNewConstraintsList.push_back(submapId);
+        }
+    }
+
+    for (typename IdToActiveSubmaps::iterator it = activeSubmaps.begin(); it != activeSubmaps.end();)
+    {
+        auto& submapData = it->second;
+        if (submapData.type == Type::LOST)
+            it = activeSubmaps.erase(it);
+        else
+            it++;
+    }
+
+    for (std::vector<int>::const_iterator it = createNewConstraintsList.begin(); it != createNewConstraintsList.end(); ++it)
+    {
+        int dataId = *it;
+        ActiveSubmapData newSubmapData;
+        newSubmapData.trackingAttempts = 0;
+        newSubmapData.type             = Type::LOOP_CLOSURE;
+        activeSubmaps[dataId]          = newSubmapData;
+    }
+
+    if (shouldCreateSubmap(_frameId))
+    {
+        Ptr<SubmapT> currActiveSubmap = getCurrentSubmap();
+        Affine3f newSubmapPose        = currActiveSubmap->pose * currActiveSubmap->cameraPose;
+        int submapId                  = createNewSubmap(false, _frameId, newSubmapPose);
+        auto newSubmap                = getSubmap(submapId);
+        newSubmap->frame              = _frame;
+    }
+
+    return mapUpdated;
+}
+
+template<typename MatType>
+Ptr<detail::PoseGraph> SubmapManager<MatType>::MapToPoseGraph()
+{
+    Ptr<detail::PoseGraph> localPoseGraph = detail::PoseGraph::create();
+
+    for(const auto& currSubmap : submapList)
+    {
+        const typename SubmapT::Constraints& constraintList = currSubmap->constraints;
+        for(const auto& currConstraintPair : constraintList)
+        {
+            // TODO: Handle case with duplicate constraints A -> B and B -> A
+            /* Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); */
+            Matx66f informationMatrix = Matx66f::eye();
+            localPoseGraph->addEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix);
+        }
+    }
+
+    for(const auto& currSubmap : submapList)
+    {
+        localPoseGraph->addNode(currSubmap->id, currSubmap->pose, (currSubmap->id == 0));
+    }
+
+    return localPoseGraph;
+}
+
+template <typename MatType>
+void SubmapManager<MatType>::PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph)
+{
+    for(const auto& currSubmap : submapList)
+    {
+        Affine3d pose = updatedPoseGraph->getNodePose(currSubmap->id);
+        if(!updatedPoseGraph->isNodeFixed(currSubmap->id))
+            currSubmap->pose = pose;
+    }
+}
+
+}  // namespace detail
+}  // namespace cv
+
+#endif // include guard

modules/ptcloud/include/opencv2/ptcloud/odometry.hpp

@@ -0,0 +1,112 @@
+// 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_ODOMETRY_HPP
+#define OPENCV_3D_ODOMETRY_HPP
+
+#include <opencv2/core.hpp>
+
+#include "odometry_frame.hpp"
+#include "odometry_settings.hpp"
+
+namespace cv
+{
+
+/** These constants are used to set a type of data which odometry will use
+* @param DEPTH     only depth data
+* @param RGB       only rgb image
+* @param RGB_DEPTH depth and rgb data simultaneously
+*/
+enum class OdometryType
+{
+    DEPTH     = 0,
+    RGB       = 1,
+    RGB_DEPTH = 2
+};
+
+/** These constants are used to set the speed and accuracy of odometry
+* @param COMMON accurate but not so fast
+* @param FAST   less accurate but faster
+*/
+enum class OdometryAlgoType
+{
+    COMMON = 0,
+    FAST = 1
+};
+
+class CV_EXPORTS_W Odometry
+{
+public:
+    CV_WRAP Odometry();
+    CV_WRAP explicit Odometry(OdometryType otype);
+    CV_WRAP Odometry(OdometryType otype, const OdometrySettings& settings, OdometryAlgoType algtype);
+    ~Odometry();
+
+    /** Prepare frame for odometry calculation
+     * @param frame odometry prepare this frame as src frame and dst frame simultaneously
+     */
+    CV_WRAP void prepareFrame(OdometryFrame& frame) const;
+
+    /** Prepare frame for odometry calculation
+     * @param srcFrame frame will be prepared as src frame ("original" image)
+     * @param dstFrame frame will be prepared as dsr frame ("rotated" image)
+     */
+    CV_WRAP void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const;
+
+    /** Compute Rigid Transformation between two frames so that Rt * src = dst
+     * Both frames, source and destination, should have been prepared by calling prepareFrame() first
+     *
+     * @param srcFrame src frame ("original" image)
+     * @param dstFrame dst frame ("rotated" image)
+     * @param Rt Rigid transformation, which will be calculated, in form:
+     * { R_11 R_12 R_13 t_1
+     *   R_21 R_22 R_23 t_2
+     *   R_31 R_32 R_33 t_3
+     *   0    0    0    1  }
+     * @return true on success, false if failed to find the transformation
+     */
+    CV_WRAP bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const;
+    /**
+     * @brief Compute Rigid Transformation between two frames so that Rt * src = dst
+     *
+     * @param srcDepth source depth ("original" image)
+     * @param dstDepth destination depth ("rotated" image)
+     * @param Rt Rigid transformation, which will be calculated, in form:
+     * { R_11 R_12 R_13 t_1
+     *   R_21 R_22 R_23 t_2
+     *   R_31 R_32 R_33 t_3
+     *   0    0    0    1  }
+     * @return true on success, false if failed to find the transformation
+     */
+    CV_WRAP bool compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const;
+    /**
+     * @brief Compute Rigid Transformation between two frames so that Rt * src = dst
+     *
+     * @param srcDepth source depth ("original" image)
+     * @param srcRGB source RGB
+     * @param dstDepth destination depth ("rotated" image)
+     * @param dstRGB destination RGB
+     * @param Rt Rigid transformation, which will be calculated, in form:
+     * { R_11 R_12 R_13 t_1
+     *   R_21 R_22 R_23 t_2
+     *   R_31 R_32 R_33 t_3
+     *   0    0    0    1  }
+     * @return true on success, false if failed to find the transformation
+     */
+    CV_WRAP bool compute(InputArray srcDepth, InputArray srcRGB, InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const;
+
+    /**
+     * @brief Get the normals computer object used for normals calculation (if presented).
+     * The normals computer is generated at first need during prepareFrame when normals are required for the ICP algorithm
+     * but not presented by a user. Re-generated each time the related settings change or a new frame arrives with the different size.
+     */
+    CV_WRAP Ptr<RgbdNormals> getNormalsComputer() const;
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
+};
+
+}
+#endif //OPENCV_3D_ODOMETRY_HPP

modules/ptcloud/include/opencv2/ptcloud/odometry_frame.hpp

@@ -0,0 +1,108 @@
+// 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_ODOMETRY_FRAME_HPP
+#define OPENCV_3D_ODOMETRY_FRAME_HPP
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+/** Indicates what pyramid is to access using getPyramidAt() method:
+*/
+
+enum OdometryFramePyramidType
+{
+    PYR_IMAGE = 0,    //!< The pyramid of grayscale images
+    PYR_DEPTH = 1,    //!< The pyramid of depth images
+    PYR_MASK = 2,     //!< The pyramid of masks
+    PYR_CLOUD = 3,    //!< The pyramid of point clouds, produced from the pyramid of depths
+    PYR_DIX = 4,      //!< The pyramid of dI/dx derivative images
+    PYR_DIY = 5,      //!< The pyramid of dI/dy derivative images
+    PYR_TEXMASK = 6,  //!< The pyramid of "textured" masks (i.e. additional masks for normals or grayscale images)
+    PYR_NORM = 7,     //!< The pyramid of normals
+    PYR_NORMMASK = 8, //!< The pyramid of normals masks
+    N_PYRAMIDS
+};
+
+/**
+ * @brief An object that keeps per-frame data for Odometry algorithms from user-provided images to algorithm-specific precalculated data.
+ * When not empty, it contains a depth image, a mask of valid pixels and a set of pyramids generated from that data.
+ * A BGR/Gray image and normals are optional.
+ * OdometryFrame is made to be used together with Odometry class to reuse precalculated data between Rt data calculations.
+ * A correct way to do that is to call Odometry::prepareFrames() on prev and next frames and then pass them to Odometry::compute() method.
+ */
+class CV_EXPORTS_W_SIMPLE OdometryFrame
+{
+public:
+    /**
+     * @brief Construct a new OdometryFrame object. All non-empty images should have the same size.
+     *
+     * @param depth A depth image, should be CV_8UC1
+     * @param image An BGR or grayscale image (or noArray() if it's not required for used ICP algorithm).
+     * Should be CV_8UC3 or CV_8C4 if it's BGR image or CV_8UC1 if it's grayscale. If it's BGR then it's converted to grayscale
+     * image automatically.
+     * @param mask A user-provided mask of valid pixels, should be CV_8UC1
+     * @param normals A user-provided normals to the depth surface, should be CV_32FC4
+     */
+    CV_WRAP explicit OdometryFrame(InputArray depth = noArray(), InputArray image = noArray(), InputArray mask = noArray(), InputArray normals = noArray());
+    ~OdometryFrame() {};
+
+    /**
+     * @brief Get the original user-provided BGR/Gray image
+     *
+     * @param image Output image
+     */
+    CV_WRAP void getImage(OutputArray image) const;
+    /**
+     * @brief Get the gray image generated from the user-provided BGR/Gray image
+     *
+     * @param image Output image
+     */
+    CV_WRAP void getGrayImage(OutputArray image) const;
+    /**
+     * @brief Get the original user-provided depth image
+     *
+     * @param depth Output image
+     */
+    CV_WRAP void getDepth(OutputArray depth) const;
+    /**
+     * @brief Get the depth image generated from the user-provided one after conversion, rescale or filtering for ICP algorithm needs
+     *
+     * @param depth Output image
+     */
+    CV_WRAP void getProcessedDepth(OutputArray depth) const;
+    /**
+     * @brief Get the valid pixels mask generated for the ICP calculations intersected with the user-provided mask
+     *
+     * @param mask Output image
+     */
+    CV_WRAP void getMask(OutputArray mask) const;
+    /**
+     * @brief Get the normals image either generated for the ICP calculations or user-provided
+     *
+     * @param normals Output image
+     */
+    CV_WRAP void getNormals(OutputArray normals) const;
+
+    /**
+     * @brief Get the amount of levels in pyramids (all of them if not empty should have the same number of levels)
+     * or 0 if no pyramids were prepared yet
+     */
+    CV_WRAP int getPyramidLevels() const;
+    /**
+     * @brief Get the image generated for the ICP calculations from one of the pyramids specified by pyrType. Returns empty image if
+     * the pyramid is empty or there's no such pyramid level
+     *
+     * @param img Output image
+     * @param pyrType Type of pyramid
+     * @param level Level in the pyramid
+     */
+    CV_WRAP void getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const;
+
+    class Impl;
+    Ptr<Impl> impl;
+};
+}
+#endif // !OPENCV_3D_ODOMETRY_FRAME_HPP

modules/ptcloud/include/opencv2/ptcloud/odometry_settings.hpp

@@ -0,0 +1,65 @@
+// 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_ODOMETRY_SETTINGS_HPP
+#define OPENCV_3D_ODOMETRY_SETTINGS_HPP
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+
+class CV_EXPORTS_W_SIMPLE OdometrySettings
+{
+public:
+    CV_WRAP OdometrySettings();
+    OdometrySettings(const OdometrySettings&);
+    OdometrySettings& operator=(const OdometrySettings&);
+    ~OdometrySettings() {};
+    CV_WRAP void setCameraMatrix(InputArray val);
+    CV_WRAP void getCameraMatrix(OutputArray val) const;
+    CV_WRAP void setIterCounts(InputArray val);
+    CV_WRAP void getIterCounts(OutputArray val) const;
+
+    CV_WRAP void  setMinDepth(float val);
+    CV_WRAP float getMinDepth() const;
+    CV_WRAP void  setMaxDepth(float val);
+    CV_WRAP float getMaxDepth() const;
+    CV_WRAP void  setMaxDepthDiff(float val);
+    CV_WRAP float getMaxDepthDiff() const;
+    CV_WRAP void  setMaxPointsPart(float val);
+    CV_WRAP float getMaxPointsPart() const;
+
+    CV_WRAP void setSobelSize(int val);
+    CV_WRAP int  getSobelSize() const;
+    CV_WRAP void   setSobelScale(double val);
+    CV_WRAP double getSobelScale() const;
+
+    CV_WRAP void setNormalWinSize(int val);
+    CV_WRAP int  getNormalWinSize() const;
+    CV_WRAP void setNormalDiffThreshold(float val);
+    CV_WRAP float getNormalDiffThreshold() const;
+    CV_WRAP void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm);
+    CV_WRAP RgbdNormals::RgbdNormalsMethod getNormalMethod() const;
+
+    CV_WRAP void  setAngleThreshold(float val);
+    CV_WRAP float getAngleThreshold() const;
+    CV_WRAP void  setMaxTranslation(float val);
+    CV_WRAP float getMaxTranslation() const;
+    CV_WRAP void  setMaxRotation(float val);
+    CV_WRAP float getMaxRotation() const;
+
+    CV_WRAP void  setMinGradientMagnitude(float val);
+    CV_WRAP float getMinGradientMagnitude() const;
+    CV_WRAP void setMinGradientMagnitudes(InputArray val);
+    CV_WRAP void getMinGradientMagnitudes(OutputArray val) const;
+
+    class Impl;
+
+private:
+    Ptr<Impl> impl;
+};
+
+}
+#endif //OPENCV_3D_ODOMETRY_SETTINGS_HPP

modules/ptcloud/include/opencv2/ptcloud/volume.hpp

@@ -0,0 +1,172 @@
+// 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_HPP
+#define OPENCV_3D_VOLUME_HPP
+
+#include "volume_settings.hpp"
+
+#include "opencv2/ptcloud/odometry_frame.hpp"
+#include "opencv2/core/affine.hpp"
+
+namespace cv
+{
+
+class CV_EXPORTS_W Volume
+{
+public:
+    /** @brief Constructor of custom volume.
+    * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
+    * @param settings the custom settings for volume.
+    */
+    CV_WRAP explicit Volume(VolumeType vtype = VolumeType::TSDF,
+                            const VolumeSettings& settings = VolumeSettings(VolumeType::TSDF));
+    ~Volume();
+
+    /** @brief Integrates the input data to the volume.
+
+    Camera intrinsics are taken from volume settings structure.
+
+    * @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.
+    */
+    CV_WRAP_AS(integrateFrame) void integrate(const OdometryFrame& frame, InputArray pose);
+
+    /** @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.
+    */
+    CV_WRAP_AS(integrate) 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.
+    */
+    CV_WRAP_AS(integrateColor) void integrate(InputArray depth, InputArray image, InputArray pose);
+
+    // Python bindings do not process noArray() default argument correctly, so the raycast() method is repeated several times
+
+    /** @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.
+    */
+    CV_WRAP void raycast(InputArray cameraPose, OutputArray points, OutputArray normals) 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).
+    */
+    CV_WRAP_AS(raycastColor) void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) 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 K camera raycast intrinsics
+    * @param points image to store rendered points.
+    * @param normals image to store rendered normals corresponding to points.
+    */
+    CV_WRAP_AS(raycastEx) void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals) 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 K camera raycast intrinsics
+    * @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).
+    */
+    CV_WRAP_AS(raycastExColor) void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals, OutputArray colors) 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.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP 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).
+    */
+    CV_WRAP void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const;
+
+    /** @brief clear all data in volume.
+    */
+    CV_WRAP void reset();
+
+    //TODO: remove this
+    /** @brief return visible blocks in volume.
+    */
+    CV_WRAP int getVisibleBlocks() const;
+
+    /** @brief return number of volume units in volume.
+    */
+    CV_WRAP size_t getTotalVolumeUnits() const;
+
+    enum BoundingBoxPrecision
+    {
+        VOLUME_UNIT = 0,
+        VOXEL = 1
+    };
+
+    /**
+     * @brief Gets bounding box in volume coordinates with given precision:
+     * VOLUME_UNIT - up to volume unit
+     * VOXEL - up to voxel (currently not supported)
+     * @param bb 6-float 1d array containing (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
+     * @param precision bounding box calculation precision
+     */
+    CV_WRAP void getBoundingBox(OutputArray bb, int precision) const;
+
+    /**
+     * @brief Enables or disables new volume unit allocation during integration.
+     * Makes sense for HashTSDF only.
+     */
+    CV_WRAP void setEnableGrowth(bool v);
+    /**
+     * @brief Returns if new volume units are allocated during integration or not.
+     * Makes sense for HashTSDF only.
+     */
+    CV_WRAP bool getEnableGrowth() const;
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
+};
+
+}  // namespace cv
+#endif // include guard

modules/ptcloud/include/opencv2/ptcloud/volume_settings.hpp

@@ -0,0 +1,208 @@
+// 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/ptcloud/volume.hpp>
+
+namespace cv
+{
+
+enum class VolumeType
+{
+    TSDF = 0,
+    HashTSDF = 1,
+    ColorTSDF = 2
+};
+
+
+class CV_EXPORTS_W_SIMPLE VolumeSettings
+{
+public:
+    /** @brief Constructor of settings for custom Volume type.
+    * @param volumeType volume type.
+    */
+    CV_WRAP explicit VolumeSettings(VolumeType volumeType = VolumeType::TSDF);
+
+    VolumeSettings(const VolumeSettings& vs);
+    VolumeSettings& operator=(const VolumeSettings&);
+    ~VolumeSettings();
+
+    /** @brief Sets the width of the image for integration.
+    * @param val input value.
+    */
+    CV_WRAP void setIntegrateWidth(int val);
+
+    /** @brief Returns the width of the image for integration.
+    */
+    CV_WRAP int getIntegrateWidth() const;
+
+    /** @brief Sets the height of the image for integration.
+    * @param val input value.
+    */
+    CV_WRAP void setIntegrateHeight(int val);
+
+    /** @brief Returns the height of the image for integration.
+    */
+    CV_WRAP int getIntegrateHeight() const;
+
+    /** @brief Sets the width of the raycasted image, used when user does not provide it at raycast() call.
+    * @param val input value.
+    */
+    CV_WRAP void setRaycastWidth(int val);
+
+    /** @brief Returns the width of the raycasted image, used when user does not provide it at raycast() call.
+    */
+    CV_WRAP int getRaycastWidth() const;
+
+    /** @brief Sets the height of the raycasted image, used when user does not provide it at raycast() call.
+    * @param val input value.
+    */
+    CV_WRAP void setRaycastHeight(int val);
+
+    /** @brief Returns the height of the raycasted image, used when user does not provide it at raycast() call.
+    */
+    CV_WRAP int getRaycastHeight() const;
+
+    /** @brief Sets depth factor, witch is the number for depth scaling.
+    * @param val input value.
+    */
+    CV_WRAP void setDepthFactor(float val);
+
+    /** @brief Returns depth factor, witch is the number for depth scaling.
+    */
+    CV_WRAP float getDepthFactor() const;
+
+    /** @brief Sets the size of voxel.
+    * @param val input value.
+    */
+    CV_WRAP void setVoxelSize(float val);
+
+    /** @brief Returns the size of voxel.
+    */
+    CV_WRAP float getVoxelSize() const;
+
+    /** @brief Sets TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
+    * @param val input value.
+    */
+    CV_WRAP void setTsdfTruncateDistance(float val);
+
+    /** @brief Returns TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
+    */
+    CV_WRAP float getTsdfTruncateDistance() const;
+
+    /** @brief Sets threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
+    * @param val input value.
+    */
+    CV_WRAP void setMaxDepth(float val);
+
+    /** @brief Returns threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP void setRaycastStepFactor(float val);
+
+    /** @brief Returns length of single raycast step.
+        Describes the percentage of voxel length that is skipped per march.
+    */
+    CV_WRAP float getRaycastStepFactor() const;
+
+    /** @brief Sets volume pose.
+    * @param val input value.
+    */
+    CV_WRAP void setVolumePose(InputArray val);
+
+    /** @brief Sets volume pose.
+    * @param val output value.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP void getVolumeResolution(OutputArray val) const;
+
+    /** @brief Returns 3 integers representing strides by x, y and z dimension.
+        Can be used to iterate over raw volume unit data.
+    * @param val output value.
+    */
+    CV_WRAP void getVolumeStrides(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.
+    */
+    CV_WRAP 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.
+    */
+    CV_WRAP void getCameraIntegrateIntrinsics(OutputArray val) const;
+
+    /** @brief Sets camera intrinsics for raycast image which, used when user does not provide them at raycast() call.
+    * 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.
+    */
+    CV_WRAP void setCameraRaycastIntrinsics(InputArray val);
+
+    /** @brief Returns camera intrinsics for raycast image, used when user does not provide them at raycast() call.
+    * 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.
+    */
+    CV_WRAP void getCameraRaycastIntrinsics(OutputArray val) const;
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
+};
+
+}
+
+#endif // !OPENCV_3D_VOLUME_SETTINGS_HPP

modules/ptcloud/misc/python/test_iomesh.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import os, numpy
+import math
+import unittest
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class raster_test(NewOpenCVTests):
+
+    def test_loadCloud(self):
+        fin = self.find_file("pointcloudio/orig.obj")
+        points, normals, colors = cv.loadPointCloud(fin)
+
+        if points.shape != (8, 1, 3):
+            self.fail('point array should be 8x1x3')
+        if normals.shape != (6, 1, 3):
+            self.fail('normals array should be 6x1x3')
+        if colors.shape != (8, 1, 3):
+            self.fail('colors array should be 8x1x3')
+
+    def test_loadMesh(self):
+        fin = self.find_file("pointcloudio/orig.obj")
+        points, indices, normals, colors, texCoords = cv.loadMesh(fin)
+        goodShapes = [(1, 18, 3), (18, 1, 3)]
+        errorMsg = "%s array should be 18x1x3 or 1x18x3"
+        for a, s in [(points, 'points'), (normals, 'normals'), (colors, 'colors')]:
+            if a.shape not in goodShapes:
+                self.fail(errorMsg % s)
+
+        if texCoords.shape not in [(1, 18, 2), (18, 1, 2), (18, 2)]:
+            self.fail('texture coordinates array should be 1x18x2 or 18x1x2')
+        if isinstance(indices, numpy.ndarray):
+            if indices.shape not in [(1, 6, 3), (6, 1, 3)]:
+                self.fail('indices array should be 1x6x3 or 6x1x3')
+        elif isinstance(indices, list) or isinstance(indices, tuple):
+            for i in indices:
+                if len(indices) != 6 or i.shape != (1, 3):
+                    self.fail('indices array should be 1x6x3 or 6x1x3')
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/misc/python/test_octree.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+
+import os
+import numpy as np
+import math
+import unittest
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class octree_test(NewOpenCVTests):
+    def test_octree_basic_test(self):
+        pointCloudSize = 1000
+        resolution = 0.0001
+        scale = 1 << 20
+
+        pointCloud = np.random.randint(-scale, scale, size=(pointCloudSize, 3)) * (10.0 / scale)
+        pointCloud = pointCloud.astype(np.float32)
+
+        octree = cv.Octree_createWithResolution(resolution, pointCloud)
+
+        restPoint = np.random.randint(-scale, scale, size=(1, 3)) * (10.0 / scale)
+        restPoint = [restPoint[0, 0], restPoint[0, 1], restPoint[0, 2]]
+
+        self.assertTrue(octree.isPointInBound(restPoint))
+        self.assertFalse(octree.deletePoint(restPoint))
+        self.assertTrue(octree.insertPoint(restPoint))
+        self.assertTrue(octree.deletePoint(restPoint))
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/misc/python/test_odometry.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python
+
+import numpy as np
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class odometry_test(NewOpenCVTests):
+    def commonOdometryTest(self, needRgb, otype, algoType, useFrame):
+        depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32)
+        if needRgb:
+            rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR)
+        radian = np.radians(1)
+        Rt_warp = np.array(
+            [[np.cos(radian), -np.sin(radian), 0],
+            [np.sin(radian), np.cos(radian), 0],
+            [0, 0, 1]], dtype=np.float32
+        )
+        Rt_curr = np.array(
+            [[np.cos(radian), -np.sin(radian), 0, 0],
+            [np.sin(radian), np.cos(radian), 0, 0],
+            [0, 0, 1, 0],
+            [0, 0, 0, 1]], dtype=np.float32
+        )
+        Rt_res = np.zeros((4, 4))
+
+        if otype is not None:
+            settings = cv.OdometrySettings()
+            odometry = cv.Odometry(otype, settings, algoType)
+        else:
+            odometry = cv.Odometry()
+
+        warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480))
+        if needRgb:
+            warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480))
+
+        if useFrame:
+            if needRgb:
+                srcFrame = cv.OdometryFrame(depth, rgb)
+                dstFrame = cv.OdometryFrame(warped_depth, warped_rgb)
+            else:
+                srcFrame = cv.OdometryFrame(depth)
+                dstFrame = cv.OdometryFrame(warped_depth)
+            odometry.prepareFrames(srcFrame, dstFrame)
+            isCorrect = odometry.compute(srcFrame, dstFrame, Rt_res)
+        else:
+            if needRgb:
+                isCorrect = odometry.compute(depth, rgb, warped_depth, warped_rgb, Rt_res)
+            else:
+                isCorrect = odometry.compute(depth, warped_depth, Rt_res)
+
+        res = np.absolute(Rt_curr - Rt_res).sum()
+        eps = 0.15
+        self.assertLessEqual(res, eps)
+        self.assertTrue(isCorrect)
+
+    def test_OdometryDefault(self):
+        self.commonOdometryTest(False, None, None, False)
+
+    def test_OdometryDefaultFrame(self):
+        self.commonOdometryTest(False, None, None, True)
+
+    def test_OdometryDepth(self):
+        self.commonOdometryTest(False, cv.OdometryType_DEPTH, cv.OdometryAlgoType_COMMON, False)
+
+    def test_OdometryDepthFast(self):
+        self.commonOdometryTest(False, cv.OdometryType_DEPTH, cv.OdometryAlgoType_FAST, False)
+
+    def test_OdometryDepthFrame(self):
+        self.commonOdometryTest(False, cv.OdometryType_DEPTH, cv.OdometryAlgoType_COMMON, True)
+
+    def test_OdometryDepthFastFrame(self):
+        self.commonOdometryTest(False, cv.OdometryType_DEPTH, cv.OdometryAlgoType_FAST, True)
+
+    def test_OdometryRGB(self):
+        self.commonOdometryTest(True, cv.OdometryType_RGB, cv.OdometryAlgoType_COMMON, False)
+
+    def test_OdometryRGBFrame(self):
+        self.commonOdometryTest(True, cv.OdometryType_RGB, cv.OdometryAlgoType_COMMON, True)
+
+    def test_OdometryRGB_Depth(self):
+        self.commonOdometryTest(True, cv.OdometryType_RGB_DEPTH, cv.OdometryAlgoType_COMMON, False)
+
+    def test_OdometryRGB_DepthFrame(self):
+        self.commonOdometryTest(True, cv.OdometryType_RGB_DEPTH, cv.OdometryAlgoType_COMMON, True)
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/misc/python/test_raster.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import os, numpy
+import math
+import unittest
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+def lookAtMatrixCal(position, lookat, upVector):
+    tmp = position - lookat
+    norm = numpy.linalg.norm(tmp)
+    w = tmp / norm
+    tmp = numpy.cross(upVector, w)
+    norm = numpy.linalg.norm(tmp)
+    u = tmp / norm
+    v = numpy.cross(w, u)
+    res = numpy.array([
+        [u[0], u[1], u[2],   0],
+        [v[0], v[1], v[2],   0],
+        [w[0], w[1], w[2],   0],
+        [0,    0,    0,   1.0]
+    ], dtype=numpy.float32)
+    translate = numpy.array([
+        [1.0,   0,   0, -position[0]],
+        [0, 1.0,   0, -position[1]],
+        [0,   0, 1.0, -position[2]],
+        [0,   0,   0,          1.0]
+    ], dtype=numpy.float32)
+    res = numpy.matmul(res, translate)
+    return res
+
+class raster_test(NewOpenCVTests):
+
+    def prepareData(self):
+        self.vertices = numpy.array([
+            [ 2.0,  0.0, -2.0],
+            [ 0.0, -6.0, -2.0],
+            [-2.0,  0.0, -2.0],
+            [ 3.5, -1.0, -5.0],
+            [ 2.5, -2.5, -5.0],
+            [-1.0,  1.0, -5.0],
+            [-6.5, -1.0, -3.0],
+            [-2.5, -2.0, -3.0],
+            [ 1.0,  1.0, -5.0],
+        ], dtype=numpy.float32)
+
+        self.indices = numpy.array([ [0, 1, 2], [3, 4, 5], [6, 7, 8]], dtype=int)
+
+        col1 = [ 185.0,  238.0,  217.0 ]
+        col2 = [ 238.0,  217.0,  185.0 ]
+        col3 = [ 238.0,   10.0,  150.0 ]
+
+        self.colors = numpy.array([
+            col1, col2, col3,
+            col2, col3, col1,
+            col3, col1, col2,
+        ], dtype=numpy.float32)
+
+        self.colors = self.colors / 255.0
+
+        self.zNear = 0.1
+        self.zFar = 50.0
+        self.fovy = 45.0 * math.pi / 180.0
+
+        position = numpy.array([0.0, 0.0, 5.0], dtype=numpy.float32)
+        lookat   = numpy.array([0.0, 0.0, 0.0], dtype=numpy.float32)
+        upVector = numpy.array([0.0, 1.0, 0.0], dtype=numpy.float32)
+        self.cameraPose = lookAtMatrixCal(position, lookat, upVector)
+
+        self.depth_buf = numpy.ones((240, 320), dtype=numpy.float32) * self.zFar
+        self.color_buf = numpy.zeros((240, 320, 3), dtype=numpy.float32)
+
+        self.settings = cv.TriangleRasterizeSettings().setShadingType(cv.RASTERIZE_SHADING_SHADED)
+        self.settings = self.settings.setCullingMode(cv.RASTERIZE_CULLING_NONE)
+
+    def compareResults(self, needRgb, needDepth):
+        if needDepth:
+            depth = self.get_sample('rendering/depth_image_Clipping_320x240_CullNone.png', cv.IMREAD_ANYDEPTH).astype(numpy.float32)
+            depthFactor = 1000.0
+            diff = depth/depthFactor - self.depth_buf
+            norm = numpy.linalg.norm(diff)
+            self.assertLessEqual(norm, 356.0)
+
+        if needRgb:
+            rgb = self.get_sample('rendering/example_image_Clipping_320x240_CullNone_Shaded.png', cv.IMREAD_ANYCOLOR)
+            diff = rgb/255.0 - self.color_buf
+            norm = numpy.linalg.norm(diff)
+            self.assertLessEqual(norm, 11.62)
+
+    def test_rasterizeBoth(self):
+        self.prepareData()
+        self.color_buf, self.depth_buf = cv.triangleRasterize(self.vertices, self.indices, self.colors, self.color_buf, self.depth_buf,
+                                                              self.cameraPose, self.fovy, self.zNear, self.zFar, self.settings)
+        self.compareResults(needRgb=True, needDepth=True)
+
+    def test_rasterizeDepth(self):
+        self.prepareData()
+        self.depth_buf = cv.triangleRasterizeDepth(self.vertices, self.indices, self.depth_buf,
+                                                   self.cameraPose, self.fovy, self.zNear, self.zFar, self.settings)
+        self.compareResults(needRgb=False, needDepth=True)
+
+    def test_rasterizeColor(self):
+        self.prepareData()
+        self.color_buf = cv.triangleRasterizeColor(self.vertices, self.indices, self.colors, self.color_buf,
+                                                   self.cameraPose, self.fovy, self.zNear, self.zFar, self.settings)
+        self.compareResults(needRgb=True, needDepth=False)
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/misc/python/test_rgbd.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python
+
+# Python 2/3 compatibility
+from __future__ import print_function
+
+import os, numpy
+import unittest
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class rgbd_test(NewOpenCVTests):
+
+    def test_computeRgbdPlane(self):
+
+        depth_image = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH)
+        if depth_image is None:
+            raise unittest.SkipTest("Missing files with test data")
+
+        K = numpy.array([[525, 0, 320.5], [0, 525, 240.5], [0, 0, 1]])
+        points3d = cv.depthTo3d(depth_image, K)
+        normals = cv.RgbdNormals_create(480, 640, cv.CV_32F, K).apply(points3d)
+        _, planes_coeff = cv.findPlanes(points3d, normals, numpy.array([]), numpy.array([]), 40, 1600, 0.01, 0, 0, 0, cv.RGBD_PLANE_METHOD_DEFAULT)
+
+        planes_coeff_expected = \
+        numpy.asarray([[[-0.02447728, -0.8678335 , -0.49625182,  4.02800846]],
+                        [[-0.05055107, -0.86144137, -0.50533485,  3.95456314]],
+                        [[-0.03294908, -0.86964548, -0.49257591,  3.97052431]],
+                        [[-0.02886586, -0.87153459, -0.48948362,  7.77550507]],
+                        [[-0.04455929, -0.87659335, -0.47916424,  3.93200684]],
+                        [[-0.21514639,  0.18835169, -0.95824611,  7.59479475]],
+                        [[-0.01006953, -0.86679155, -0.49856904,  4.01355648]],
+                        [[-0.00876531, -0.87571168, -0.48275498,  3.96768975]],
+                        [[-0.06395926, -0.86951321, -0.48975089,  4.08618736]],
+                        [[-0.01403128, -0.87593341, -0.48222789,  7.74559402]],
+                        [[-0.01143177, -0.87495202, -0.4840748 ,  7.75355816]]],
+                        dtype=numpy.float32)
+
+        eps = 0.05
+        self.assertLessEqual(cv.norm(planes_coeff, planes_coeff_expected, cv.NORM_L2), eps)
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/misc/python/test_volume.py

@@ -0,0 +1,123 @@
+import numpy as np
+import cv2 as cv
+
+from tests_common import NewOpenCVTests
+
+class volume_test(NewOpenCVTests):
+    def test_VolumeDefault(self):
+        depthPath = 'cv/rgbd/depth.png'
+        depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32)
+        if depth.size <= 0:
+            raise Exception('Failed to load depth file: %s' % depthPath)
+
+        Rt = np.eye(4)
+        volume = cv.Volume()
+        volume.integrate(depth, Rt)
+
+        size = (480, 640, 4)
+        points  = np.zeros(size, np.float32)
+        normals = np.zeros(size, np.float32)
+
+        Kraycast = np.array([[525. ,   0. , 319.5],
+                             [  0. , 525. , 239.5],
+                             [  0. ,   0. ,   1. ]])
+
+        volume.raycastEx(Rt, size[0], size[1], Kraycast, points, normals)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+        volume.raycast(Rt, points, normals)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+    def test_VolumeTSDF(self):
+        depthPath = 'cv/rgbd/depth.png'
+        depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32)
+        if depth.size <= 0:
+            raise Exception('Failed to load depth file: %s' % depthPath)
+
+        Rt = np.eye(4)
+
+        settings = cv.VolumeSettings(cv.VolumeType_TSDF)
+        volume = cv.Volume(cv.VolumeType_TSDF, settings)
+        volume.integrate(depth, Rt)
+
+        size = (480, 640, 4)
+        points  = np.zeros(size, np.float32)
+        normals = np.zeros(size, np.float32)
+
+        Kraycast = settings.getCameraRaycastIntrinsics()
+        volume.raycastEx(Rt, size[0], size[1], Kraycast, points, normals)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+        volume.raycast(Rt, points, normals)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+    def test_VolumeHashTSDF(self):
+        depthPath = 'cv/rgbd/depth.png'
+        depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32)
+        if depth.size <= 0:
+            raise Exception('Failed to load depth file: %s' % depthPath)
+
+        Rt = np.eye(4)
+        settings = cv.VolumeSettings(cv.VolumeType_HashTSDF)
+        volume = cv.Volume(cv.VolumeType_HashTSDF, settings)
+        volume.integrate(depth, Rt)
+
+        size = (480, 640, 4)
+        points  = np.zeros(size, np.float32)
+        normals = np.zeros(size, np.float32)
+
+        Kraycast = settings.getCameraRaycastIntrinsics()
+        volume.raycastEx(Rt, size[0], size[1], Kraycast, points, normals)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+        volume.raycast(Rt, points, normals)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+    def test_VolumeColorTSDF(self):
+        depthPath = 'cv/rgbd/depth.png'
+        rgbPath = 'cv/rgbd/rgb.png'
+        depth = self.get_sample(depthPath, cv.IMREAD_ANYDEPTH).astype(np.float32)
+        rgb = self.get_sample(rgbPath, cv.IMREAD_ANYCOLOR).astype(np.float32)
+
+        if depth.size <= 0:
+            raise Exception('Failed to load depth file: %s' % depthPath)
+        if rgb.size <= 0:
+            raise Exception('Failed to load RGB file: %s' % rgbPath)
+
+        Rt = np.eye(4)
+        settings = cv.VolumeSettings(cv.VolumeType_ColorTSDF)
+        volume = cv.Volume(cv.VolumeType_ColorTSDF, settings)
+        volume.integrateColor(depth, rgb, Rt)
+
+        size = (480, 640, 4)
+        points  = np.zeros(size, np.float32)
+        normals = np.zeros(size, np.float32)
+        colors = np.zeros(size, np.float32)
+
+        Kraycast = settings.getCameraRaycastIntrinsics()
+        volume.raycastExColor(Rt, size[0], size[1], Kraycast, points, normals, colors)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+
+        volume.raycastColor(Rt, points, normals, colors)
+
+        self.assertEqual(points.shape, size)
+        self.assertEqual(points.shape, normals.shape)
+        self.assertEqual(points.shape, colors.shape)
+
+
+if __name__ == '__main__':
+    NewOpenCVTests.bootstrap()

modules/ptcloud/perf/perf_main.cpp

@@ -0,0 +1,7 @@
+#include "perf_precomp.hpp"
+
+#if defined(HAVE_HPX)
+    #include <hpx/hpx_main.hpp>
+#endif
+
+CV_PERF_TEST_MAIN(calib3d)

modules/ptcloud/perf/perf_precomp.hpp

@@ -0,0 +1,14 @@
+// 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_PERF_PRECOMP_HPP__
+#define __OPENCV_PERF_PRECOMP_HPP__
+
+#include "opencv2/ts.hpp"
+#include "opencv2/ptcloud.hpp"
+
+#ifdef HAVE_OPENCL
+#include <opencv2/core/ocl.hpp>
+#endif
+
+#endif

modules/ptcloud/perf/perf_rendering.cpp

@@ -0,0 +1,342 @@
+// 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
+
+#include "perf_precomp.hpp"
+
+namespace opencv_test
+{
+
+// that was easier than using CV_ENUM() macro
+namespace
+{
+using namespace cv;
+struct ShadingTypeEnum
+{
+    static const std::array<TriangleShadingType, 3> vals;
+    static const std::array<std::string, 3> svals;
+
+    ShadingTypeEnum(TriangleShadingType v = RASTERIZE_SHADING_WHITE) : val(v) {}
+    operator TriangleShadingType() const { return val; }
+    void PrintTo(std::ostream *os) const
+    {
+        int v = int(val);
+        if (v >= 0 && v < (int)vals.size())
+        {
+            *os << svals[v];
+        }
+        else
+        {
+            *os << "UNKNOWN";
+        }
+    }
+    static ::testing::internal::ParamGenerator<ShadingTypeEnum> all()
+    {
+        return ::testing::Values(ShadingTypeEnum(vals[0]),
+                                 ShadingTypeEnum(vals[1]),
+                                 ShadingTypeEnum(vals[2]));
+    }
+
+private:
+    TriangleShadingType val;
+};
+
+const std::array<TriangleShadingType, 3> ShadingTypeEnum::vals
+{
+    RASTERIZE_SHADING_WHITE,
+    RASTERIZE_SHADING_FLAT,
+    RASTERIZE_SHADING_SHADED
+};
+const std::array<std::string, 3> ShadingTypeEnum::svals
+{
+    std::string("White"),
+    std::string("Flat"),
+    std::string("Shaded")
+};
+
+static inline void PrintTo(const ShadingTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
+
+
+using namespace cv;
+struct GlCompatibleModeEnum
+{
+    static const std::array<TriangleGlCompatibleMode, 2> vals;
+    static const std::array<std::string, 2> svals;
+
+    GlCompatibleModeEnum(TriangleGlCompatibleMode v = RASTERIZE_COMPAT_DISABLED) : val(v) {}
+    operator TriangleGlCompatibleMode() const { return val; }
+    void PrintTo(std::ostream *os) const
+    {
+        int v = int(val);
+        if (v >= 0 && v < (int)vals.size())
+        {
+            *os << svals[v];
+        }
+        else
+        {
+            *os << "UNKNOWN";
+        }
+    }
+    static ::testing::internal::ParamGenerator<GlCompatibleModeEnum> all()
+    {
+        return ::testing::Values(GlCompatibleModeEnum(vals[0]),
+                                 GlCompatibleModeEnum(vals[1]));
+    }
+
+private:
+    TriangleGlCompatibleMode val;
+};
+
+const std::array<TriangleGlCompatibleMode, 2> GlCompatibleModeEnum::vals
+{
+    RASTERIZE_COMPAT_DISABLED,
+    RASTERIZE_COMPAT_INVDEPTH,
+};
+const std::array<std::string, 2> GlCompatibleModeEnum::svals
+{
+    std::string("Disabled"),
+    std::string("InvertedDepth"),
+};
+
+static inline void PrintTo(const GlCompatibleModeEnum &t, std::ostream *os) { t.PrintTo(os); }
+}
+
+enum class Outputs
+{
+    DepthOnly  = 0,
+    ColorOnly  = 1,
+    DepthColor = 2,
+};
+
+// that was easier than using CV_ENUM() macro
+namespace
+{
+using namespace cv;
+struct OutputsEnum
+{
+    static const std::array<Outputs, 3> vals;
+    static const std::array<std::string, 3> svals;
+
+    OutputsEnum(Outputs v = Outputs::DepthColor) : val(v) {}
+    operator Outputs() const { return val; }
+    void PrintTo(std::ostream *os) const
+    {
+        int v = int(val);
+        if (v >= 0 && v < (int)vals.size())
+        {
+            *os << svals[v];
+        }
+        else
+        {
+            *os << "UNKNOWN";
+        }
+    }
+    static ::testing::internal::ParamGenerator<OutputsEnum> all()
+    {
+        return ::testing::Values(OutputsEnum(vals[0]),
+                                 OutputsEnum(vals[1]),
+                                 OutputsEnum(vals[2]));
+    }
+
+private:
+    Outputs val;
+};
+
+const std::array<Outputs, 3> OutputsEnum::vals
+{
+    Outputs::DepthOnly,
+    Outputs::ColorOnly,
+    Outputs::DepthColor
+};
+const std::array<std::string, 3> OutputsEnum::svals
+{
+    std::string("DepthOnly"),
+    std::string("ColorOnly"),
+    std::string("DepthColor")
+};
+
+static inline void PrintTo(const OutputsEnum &t, std::ostream *os) { t.PrintTo(os); }
+}
+
+static Matx44d lookAtMatrixCal(const Vec3d& position, const Vec3d& lookat, const Vec3d& upVector)
+{
+    Vec3d w = cv::normalize(position - lookat);
+    Vec3d u = cv::normalize(upVector.cross(w));
+
+    Vec3d v = w.cross(u);
+
+    Matx44d res(u[0], u[1], u[2],   0,
+                v[0], v[1], v[2],   0,
+                w[0], w[1], w[2],   0,
+                   0,    0,    0,   1.0);
+
+    Matx44d translate(1.0,   0,   0, -position[0],
+                        0, 1.0,   0, -position[1],
+                        0,   0, 1.0, -position[2],
+                        0,   0,   0,          1.0);
+    res = res * translate;
+
+    return res;
+}
+
+
+static void generateNormals(const std::vector<Vec3f>& points, const std::vector<std::vector<int>>& indices,
+                            std::vector<Vec3f>& normals)
+{
+    std::vector<std::vector<Vec3f>> preNormals(points.size(), std::vector<Vec3f>());
+
+    for (const auto& tri : indices)
+    {
+        Vec3f p0 = points[tri[0]];
+        Vec3f p1 = points[tri[1]];
+        Vec3f p2 = points[tri[2]];
+
+        Vec3f cross = cv::normalize((p1 - p0).cross(p2 - p0));
+        for (int i = 0; i < 3; i++)
+        {
+            preNormals[tri[i]].push_back(cross);
+        }
+    }
+
+    normals.reserve(points.size());
+    for (const auto& pn : preNormals)
+    {
+        Vec3f sum { };
+        for (const auto& n : pn)
+        {
+            sum += n;
+        }
+        normals.push_back(cv::normalize(sum));
+    }
+}
+
+// load model once and keep it in static memory
+static void getModelOnce(const std::string& objectPath, std::vector<Vec3f>& vertices,
+                         std::vector<Vec3i>& indices, std::vector<Vec3f>& colors)
+{
+    static bool load = false;
+    static std::vector<Vec3f> vert, col;
+    static std::vector<Vec3i> ind;
+
+    if (!load)
+    {
+        std::vector<vector<int>> indvec;
+        // using per-vertex normals as colors
+        loadMesh(objectPath, vert, indvec);
+        generateNormals(vert, indvec, col);
+
+        for (const auto &vec : indvec)
+        {
+            ind.push_back({vec[0], vec[1], vec[2]});
+        }
+
+        for (auto &color : col)
+        {
+            color = Vec3f(abs(color[0]), abs(color[1]), abs(color[2]));
+        }
+
+        load = true;
+    }
+
+    vertices = vert;
+    colors = col;
+    indices = ind;
+}
+
+template<typename T>
+std::string printEnum(T v)
+{
+    std::ostringstream ss;
+    v.PrintTo(&ss);
+    return ss.str();
+}
+
+// resolution, shading type, outputs needed
+typedef perf::TestBaseWithParam<std::tuple<std::tuple<int, int>, ShadingTypeEnum, OutputsEnum, GlCompatibleModeEnum>> RenderingTest;
+
+PERF_TEST_P(RenderingTest, rasterizeTriangles, ::testing::Combine(
+    ::testing::Values(std::make_tuple(1920, 1080), std::make_tuple(1024, 768), std::make_tuple(640, 480)),
+    ShadingTypeEnum::all(),
+    OutputsEnum::all(),
+    GlCompatibleModeEnum::all()
+    ))
+{
+    auto t = GetParam();
+    auto wh = std::get<0>(t);
+    int width = std::get<0>(wh);
+    int height = std::get<1>(wh);
+    auto shadingType = std::get<1>(t);
+    auto outputs = std::get<2>(t);
+    auto glCompatibleMode = std::get<3>(t);
+
+    string objectPath = findDataFile("viz/dragon.ply");
+
+    Vec3f position = Vec3d( 1.9, 0.4, 1.3);
+    Vec3f lookat   = Vec3d( 0.0, 0.0, 0.0);
+    Vec3f upVector = Vec3d( 0.0, 1.0, 0.0);
+
+    double fovy = 45.0;
+
+    std::vector<Vec3f> vertices;
+    std::vector<Vec3i> indices;
+    std::vector<Vec3f> colors;
+
+    getModelOnce(objectPath, vertices, indices, colors);
+    if (shadingType != RASTERIZE_SHADING_WHITE)
+    {
+        // let vertices be in BGR format to avoid later color conversions
+        // mixChannels does not support in-place operation
+        cv::mixChannels(Mat(colors).clone(), colors, {0, 2, 1, 1, 2, 0});
+    }
+
+    double zNear = 0.1, zFar = 50.0;
+
+    Matx44d cameraPose = lookAtMatrixCal(position, lookat, upVector);
+    double fovYradians = fovy * (CV_PI / 180.0);
+    TriangleRasterizeSettings settings;
+    settings.setCullingMode(RASTERIZE_CULLING_CW)
+            .setShadingType(shadingType)
+            .setGlCompatibleMode(glCompatibleMode);
+
+    Mat depth_buf, color_buf;
+    while (next())
+    {
+        // Prefilled to measure pure rendering time w/o allocation and clear
+        float zMax = (glCompatibleMode == RASTERIZE_COMPAT_INVDEPTH) ? 1.f : (float)zFar;
+        depth_buf = Mat(height, width, CV_32F, zMax);
+        color_buf = Mat(height, width, CV_32FC3, Scalar::all(0));
+
+        startTimer();
+        if (outputs == Outputs::ColorOnly)
+        {
+            cv::triangleRasterizeColor(vertices, indices, colors, color_buf, cameraPose,
+                                       fovYradians, zNear, zFar, settings);
+        }
+        else if (outputs == Outputs::DepthOnly)
+        {
+            cv::triangleRasterizeDepth(vertices, indices, depth_buf, cameraPose,
+                                       fovYradians, zNear, zFar, settings);
+        }
+        else // Outputs::DepthColor
+        {
+            cv::triangleRasterize(vertices, indices, colors, color_buf, depth_buf,
+                                  cameraPose, fovYradians, zNear, zFar, settings);
+        }
+        stopTimer();
+    }
+
+    if (debugLevel > 0)
+    {
+        depth_buf.convertTo(depth_buf, CV_16U, 1000.0);
+
+        std::string shadingName = printEnum(shadingType);
+        std::string suffix = cv::format("%dx%d_%s", width, height, shadingName.c_str());
+
+        imwrite("perf_color_image_" + suffix + ".png", color_buf * 255.f);
+        imwrite("perf_depth_image_" + suffix + ".png", depth_buf);
+    }
+
+    SANITY_CHECK_NOTHING();
+}
+
+} // namespace

modules/ptcloud/perf/perf_tsdf.cpp

@@ -0,0 +1,683 @@
+// 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
+
+#include "perf_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+using namespace cv;
+
+/** Reprojects screen point to camera space given z coord. */
+struct Reprojector
+{
+    Reprojector() {}
+    inline Reprojector(Matx33f intr)
+    {
+        fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1);
+        cx = intr(0, 2), cy = intr(1, 2);
+    }
+    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;
+};
+
+template<class Scene>
+struct RenderInvoker : ParallelLoopBody
+{
+    RenderInvoker(Mat_<float>& _frame, Affine3f _pose,
+        Reprojector _reproj, float _depthFactor, bool _onlySemisphere)
+        : ParallelLoopBody(),
+        frame(_frame),
+        pose(_pose),
+        reproj(_reproj),
+        depthFactor(_depthFactor),
+        onlySemisphere(_onlySemisphere)
+    { }
+
+    virtual void operator ()(const cv::Range& r) const
+    {
+        for (int y = r.start; y < r.end; y++)
+        {
+            float* frameRow = frame[y];
+            for (int x = 0; x < frame.cols; x++)
+            {
+                float pix = 0;
+
+                Point3f orig = pose.translation();
+                // direction through pixel
+                Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f));
+                float xyt = 1.f / (screenVec.x * screenVec.x +
+                    screenVec.y * screenVec.y + 1.f);
+                Point3f dir = normalize(Vec3f(pose.rotation() * screenVec));
+                // screen space axis
+                dir.y = -dir.y;
+
+                const float maxDepth = 20.f;
+                const float maxSteps = 256;
+                float t = 0.f;
+                for (int step = 0; step < maxSteps && t < maxDepth; step++)
+                {
+                    Point3f p = orig + dir * t;
+                    float d = Scene::map(p, onlySemisphere);
+                    if (d < 0.000001f)
+                    {
+                        float depth = std::sqrt(t * t * xyt);
+                        pix = depth * depthFactor;
+                        break;
+                    }
+                    t += d;
+                }
+
+                frameRow[x] = pix;
+            }
+        }
+    }
+
+    Mat_<float>& frame;
+    Affine3f pose;
+    Reprojector reproj;
+    float depthFactor;
+    bool onlySemisphere;
+};
+
+template<class Scene>
+struct RenderColorInvoker : ParallelLoopBody
+{
+    RenderColorInvoker(Mat_<Vec3f>& _frame, Affine3f _pose,
+        Reprojector _reproj,
+        float _depthFactor, bool _onlySemisphere) : ParallelLoopBody(),
+        frame(_frame),
+        pose(_pose),
+        reproj(_reproj),
+        depthFactor(_depthFactor),
+        onlySemisphere(_onlySemisphere)
+    { }
+
+    virtual void operator ()(const cv::Range& r) const
+    {
+        for (int y = r.start; y < r.end; y++)
+        {
+            Vec3f* frameRow = frame[y];
+            for (int x = 0; x < frame.cols; x++)
+            {
+                Vec3f pix = 0;
+
+                Point3f orig = pose.translation();
+                // direction through pixel
+                Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f));
+                Point3f dir = normalize(Vec3f(pose.rotation() * screenVec));
+                // screen space axis
+                dir.y = -dir.y;
+
+                const float maxDepth = 20.f;
+                const float maxSteps = 256;
+                float t = 0.f;
+                for (int step = 0; step < maxSteps && t < maxDepth; step++)
+                {
+                    Point3f p = orig + dir * t;
+                    float d = Scene::map(p, onlySemisphere);
+                    if (d < 0.000001f)
+                    {
+                        float m = 0.25f;
+                        float p0 = float(abs(fmod(p.x, m)) > m / 2.f);
+                        float p1 = float(abs(fmod(p.y, m)) > m / 2.f);
+                        float p2 = float(abs(fmod(p.z, m)) > m / 2.f);
+
+                        pix[0] = p0 + p1;
+                        pix[1] = p1 + p2;
+                        pix[2] = p0 + p2;
+
+                        pix *= 128.f;
+                        break;
+                    }
+                    t += d;
+                }
+
+                frameRow[x] = pix;
+            }
+        }
+    }
+
+    Mat_<Vec3f>& frame;
+    Affine3f pose;
+    Reprojector reproj;
+    float depthFactor;
+    bool onlySemisphere;
+};
+
+
+struct Scene
+{
+    virtual ~Scene() {}
+    static Ptr<Scene> create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere);
+    virtual Mat_<float> depth(Affine3f pose) = 0;
+    virtual Mat_<Vec3f> rgb(Affine3f pose) = 0;
+    virtual std::vector<Affine3f> getPoses() = 0;
+};
+
+struct SemisphereScene : Scene
+{
+    const int framesPerCycle = 72;
+    const float nCycles = 0.25f;
+    const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f));
+
+    Size frameSize;
+    Matx33f intr;
+    float depthFactor;
+    bool onlySemisphere;
+
+    SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) :
+        frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere)
+    { }
+
+    static float map(Point3f p, bool onlySemisphere)
+    {
+        float plane = p.y + 0.5f;
+        Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f);
+        float sphereRadius = 0.5f;
+        float sphere = (float)cv::norm(spherePose) - sphereRadius;
+        float sphereMinusBox = sphere;
+
+        float subSphereRadius = 0.05f;
+        Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f);
+        float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius;
+
+        float res;
+        if (!onlySemisphere)
+            res = min({ sphereMinusBox, subSphere, plane });
+        else
+            res = sphereMinusBox;
+
+        return res;
+    }
+
+    Mat_<float> depth(Affine3f pose) override
+    {
+        Mat_<float> frame(frameSize);
+        Reprojector reproj(intr);
+
+        Range range(0, frame.rows);
+        parallel_for_(range, RenderInvoker<SemisphereScene>(frame, pose, reproj, depthFactor, onlySemisphere));
+
+        return frame;
+    }
+
+    Mat_<Vec3f> rgb(Affine3f pose) override
+    {
+        Mat_<Vec3f> frame(frameSize);
+        Reprojector reproj(intr);
+
+        Range range(0, frame.rows);
+        parallel_for_(range, RenderColorInvoker<SemisphereScene>(frame, pose, reproj, depthFactor, onlySemisphere));
+
+        return frame;
+    }
+
+    std::vector<Affine3f> getPoses() override
+    {
+        std::vector<Affine3f> poses;
+        for (int i = 0; i < framesPerCycle * nCycles; i++)
+        {
+            float angle = (float)(CV_2PI * i / framesPerCycle);
+            Affine3f pose;
+            pose = pose.rotate(startPose.rotation());
+            pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle);
+            pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle),
+                startPose.translation()[1],
+                startPose.translation()[2] * cos(angle)));
+            poses.push_back(pose);
+        }
+
+        return poses;
+    }
+
+};
+
+Ptr<Scene> Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere)
+{
+    return makePtr<SemisphereScene>(sz, _intr, _depthFactor, _onlySemisphere);
+}
+
+// this is a temporary solution
+// ----------------------------
+
+typedef cv::Vec4f ptype;
+typedef cv::Mat_< ptype > Points;
+typedef cv::Mat_< ptype > Colors;
+typedef Points Normals;
+typedef Size2i Size;
+
+template<int p>
+inline float specPow(float x)
+{
+    if (p % 2 == 0)
+    {
+        float v = specPow<p / 2>(x);
+        return v * v;
+    }
+    else
+    {
+        float v = specPow<(p - 1) / 2>(x);
+        return v * v * x;
+    }
+}
+
+template<>
+inline float specPow<0>(float /*x*/)
+{
+    return 1.f;
+}
+
+template<>
+inline float specPow<1>(float x)
+{
+    return x;
+}
+
+inline cv::Vec3f fromPtype(const ptype& x)
+{
+    return cv::Vec3f(x[0], x[1], x[2]);
+}
+
+inline Point3f normalize(const Vec3f& v)
+{
+    double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+    return v * (nv ? 1. / nv : 0.);
+}
+
+void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose)
+{
+    Size sz = _points.size();
+    image.create(sz, CV_8UC4);
+
+    Points  points = _points.getMat();
+    Normals normals = _normals.getMat();
+
+    Mat_<Vec4b> img = image.getMat();
+
+    Mat goods;
+    finiteMask(points, goods);
+
+    Range range(0, sz.height);
+    const int nstripes = -1;
+    parallel_for_(range, [&](const Range&)
+        {
+            for (int y = range.start; y < range.end; y++)
+            {
+                Vec4b* imgRow = img[y];
+                const ptype* ptsRow = points[y];
+                const ptype* nrmRow = normals[y];
+                const uchar* goodRow = goods.ptr<uchar>(y);
+
+                for (int x = 0; x < sz.width; x++)
+                {
+                    Point3f p = fromPtype(ptsRow[x]);
+                    Point3f n = fromPtype(nrmRow[x]);
+
+                    Vec4b color;
+
+                    if ( !goodRow[x] )
+                    {
+                        color = Vec4b(0, 32, 0, 0);
+                    }
+                    else
+                    {
+                        const float Ka = 0.3f;  //ambient coeff
+                        const float Kd = 0.5f;  //diffuse coeff
+                        const float Ks = 0.2f;  //specular coeff
+                        const int   sp = 20;  //specular power
+
+                        const float Ax = 1.f;   //ambient color,  can be RGB
+                        const float Dx = 1.f;   //diffuse color,  can be RGB
+                        const float Sx = 1.f;   //specular color, can be RGB
+                        const float Lx = 1.f;   //light color
+
+                        Point3f l = normalize(lightPose.translation() - Vec3f(p));
+                        Point3f v = normalize(-Vec3f(p));
+                        Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l));
+
+                        uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) +
+                            Lx * Ks * Sx * specPow<sp>(max(0.f, r.dot(v)))) * 255.f);
+                        color = Vec4b(ix, ix, ix, 0);
+                    }
+
+                    imgRow[x] = color;
+                }
+            }
+        }, nstripes);
+}
+void renderPointsNormalsColors(InputArray _points, InputArray, InputArray _colors, OutputArray image, Affine3f)
+{
+    Size sz = _points.size();
+    image.create(sz, CV_8UC4);
+
+    Points  points = _points.getMat();
+    Colors  colors = _colors.getMat();
+
+    Mat goods, goodp, goodc;
+    finiteMask(points, goodp);
+    finiteMask(colors, goodc);
+    goods = goodp & goodc;
+
+    Mat_<Vec4b> img = image.getMat();
+
+    Range range(0, sz.height);
+    const int nstripes = -1;
+    parallel_for_(range, [&](const Range&)
+        {
+            for (int y = range.start; y < range.end; y++)
+            {
+                Vec4b* imgRow = img[y];
+                const ptype* clrRow = colors[y];
+                const uchar* goodRow = goods.ptr<uchar>(y);
+
+                for (int x = 0; x < sz.width; x++)
+                {
+                    Point3f c = fromPtype(clrRow[x]);
+
+                    Vec4b color;
+
+                    if ( !goodRow[x] )
+                    {
+                        color = Vec4b(0, 32, 0, 0);
+                    }
+                    else
+                    {
+                        color = Vec4b((uchar)c.x, (uchar)c.y, (uchar)c.z, (uchar)0);
+                    }
+
+                    imgRow[x] = color;
+                }
+            }
+        }, nstripes);
+}
+// ----------------------------
+
+void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose)
+{
+    Mat image;
+    patchNaNs(points);
+    imshow("depth", depth * (1.f / depthFactor / 4.f));
+    renderPointsNormals(points, normals, image, lightPose);
+    imshow("render", image);
+    waitKey(100);
+}
+
+void displayColorImage(Mat depth, Mat rgb, Mat points, Mat normals, Mat colors, float depthFactor, Vec3f lightPose)
+{
+    Mat image;
+    patchNaNs(points);
+    imshow("depth", depth * (1.f / depthFactor / 4.f));
+    imshow("rgb", rgb * (1.f / 255.f));
+    renderPointsNormalsColors(points, normals, colors, image, lightPose);
+    imshow("render", image);
+    waitKey(100);
+}
+
+static const bool display = false;
+
+enum PlatformType
+{
+    CPU = 0, GPU = 1
+};
+CV_ENUM(PlatformTypeEnum, PlatformType::CPU, PlatformType::GPU);
+
+enum Sequence
+{
+    ALL = 0, FIRST = 1
+};
+CV_ENUM(SequenceEnum, Sequence::ALL, Sequence::FIRST);
+
+enum class VolumeTestSrcType
+{
+    MAT = 0,
+    ODOMETRY_FRAME = 1
+};
+
+// used to store current OpenCL status (on/off) and revert it after test is done
+// works even after exceptions thrown in test body
+struct OpenCLStatusRevert
+{
+#ifdef HAVE_OPENCL
+    OpenCLStatusRevert()
+    {
+        originalOpenCLStatus = cv::ocl::useOpenCL();
+    }
+    ~OpenCLStatusRevert()
+    {
+        cv::ocl::setUseOpenCL(originalOpenCLStatus);
+    }
+    void off()
+    {
+        cv::ocl::setUseOpenCL(false);
+    }
+    bool originalOpenCLStatus;
+#else
+    void off() { }
+#endif
+};
+
+// CV_ENUM does not support enum class types, so let's implement the class explicitly
+namespace
+{
+    struct VolumeTypeEnum
+    {
+        static const std::array<VolumeType, 3> vals;
+        static const std::array<std::string, 3> svals;
+
+        VolumeTypeEnum(VolumeType v = VolumeType::TSDF) : val(v) {}
+        operator VolumeType() const { return val; }
+        void PrintTo(std::ostream *os) const
+        {
+            int v = int(val);
+            if (v >= 0 && v < 3)
+            {
+                *os << svals[v];
+            }
+            else
+            {
+                *os << "UNKNOWN";
+            }
+        }
+        static ::testing::internal::ParamGenerator<VolumeTypeEnum> all()
+        {
+            return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2]));
+        }
+
+    private:
+        VolumeType val;
+    };
+    const std::array<VolumeType, 3> VolumeTypeEnum::vals{VolumeType::TSDF, VolumeType::HashTSDF, VolumeType::ColorTSDF};
+    const std::array<std::string, 3> VolumeTypeEnum::svals{std::string("TSDF"), std::string("HashTSDF"), std::string("ColorTSDF")};
+
+    static inline void PrintTo(const VolumeTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
+
+
+    struct VolumeTestSrcTypeEnum
+    {
+        static const std::array<VolumeTestSrcType, 2> vals;
+        static const std::array<std::string, 2> svals;
+
+        VolumeTestSrcTypeEnum(VolumeTestSrcType v = VolumeTestSrcType::MAT) : val(v) {}
+        operator VolumeTestSrcType() const { return val; }
+        void PrintTo(std::ostream *os) const
+        {
+            int v = int(val);
+            if (v >= 0 && v < 3)
+            {
+                *os << svals[v];
+            }
+            else
+            {
+                *os << "UNKNOWN";
+            }
+        }
+        static ::testing::internal::ParamGenerator<VolumeTestSrcTypeEnum> all()
+        {
+            return ::testing::Values(VolumeTestSrcTypeEnum(vals[0]), VolumeTestSrcTypeEnum(vals[1]));
+        }
+
+    private:
+        VolumeTestSrcType val;
+    };
+    const std::array<VolumeTestSrcType, 2> VolumeTestSrcTypeEnum::vals{VolumeTestSrcType::MAT, VolumeTestSrcType::ODOMETRY_FRAME};
+    const std::array<std::string, 2> VolumeTestSrcTypeEnum::svals{std::string("UMat"), std::string("OdometryFrame")};
+
+    static inline void PrintTo(const VolumeTestSrcTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
+}
+
+typedef std::tuple<PlatformTypeEnum, VolumeTypeEnum> PlatformVolumeType;
+class VolumePerfFixture : public perf::TestBaseWithParam<std::tuple<PlatformVolumeType, VolumeTestSrcTypeEnum, SequenceEnum>>
+{
+protected:
+    void SetUp() override
+    {
+        TestBase::SetUp();
+
+        auto p = GetParam();
+        gpu = (std::get<0>(std::get<0>(p)) == PlatformType::GPU);
+        volumeType = std::get<1>(std::get<0>(p));
+
+        testSrcType = std::get<1>(p);
+
+        repeat1st = (std::get<2>(p) == Sequence::FIRST);
+
+        if (!gpu)
+            oclStatus.off();
+
+        VolumeSettings vs(volumeType);
+        volume = makePtr<Volume>(volumeType, vs);
+
+        frameSize = Size(vs.getRaycastWidth(), vs.getRaycastHeight());
+        Matx33f intrIntegrate;
+        vs.getCameraIntegrateIntrinsics(intrIntegrate);
+        vs.getCameraRaycastIntrinsics(intrRaycast);
+        bool onlySemisphere = false;
+        depthFactor = vs.getDepthFactor();
+        scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
+        poses = scene->getPoses();
+    }
+
+    bool gpu;
+    VolumeType volumeType;
+    VolumeTestSrcType testSrcType;
+    bool repeat1st;
+
+    OpenCLStatusRevert oclStatus;
+
+    Ptr<Volume> volume;
+    Size frameSize;
+    Matx33f intrRaycast;
+    Ptr<Scene> scene;
+    std::vector<Affine3f> poses;
+    float depthFactor;
+};
+
+
+PERF_TEST_P_(VolumePerfFixture, integrate)
+{
+    for (size_t i = 0; i < (repeat1st ? 1 : poses.size()); i++)
+    {
+        Matx44f pose = poses[i].matrix;
+        Mat depth = scene->depth(pose);
+        Mat rgb = scene->rgb(pose);
+        UMat urgb, udepth;
+        depth.copyTo(udepth);
+        rgb.copyTo(urgb);
+        OdometryFrame odf(udepth, urgb);
+
+        bool done = false;
+        while (repeat1st ? next() : !done)
+        {
+            startTimer();
+            if (testSrcType == VolumeTestSrcType::MAT)
+                if (volumeType == VolumeType::ColorTSDF)
+                    volume->integrate(udepth, urgb, pose);
+                else
+                    volume->integrate(udepth, pose);
+            else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
+                volume->integrate(odf, pose);
+            stopTimer();
+
+            // perf check makes sense only for identical states
+            if (repeat1st)
+                volume->reset();
+
+            done = true;
+        }
+    }
+    SANITY_CHECK_NOTHING();
+}
+
+
+PERF_TEST_P_(VolumePerfFixture, raycast)
+{
+    for (size_t i = 0; i < (repeat1st ? 1 : poses.size()); i++)
+    {
+        Matx44f pose = poses[i].matrix;
+        Mat depth = scene->depth(pose);
+        Mat rgb = scene->rgb(pose);
+        UMat urgb, udepth;
+        depth.copyTo(udepth);
+        rgb.copyTo(urgb);
+
+        OdometryFrame odf(udepth, urgb);
+
+        if (testSrcType == VolumeTestSrcType::MAT)
+            if (volumeType == VolumeType::ColorTSDF)
+                volume->integrate(udepth, urgb, pose);
+            else
+                volume->integrate(udepth, pose);
+        else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
+            volume->integrate(odf, pose);
+
+        UMat upoints, unormals, ucolors;
+
+        bool done = false;
+        while (repeat1st ? next() : !done)
+        {
+            startTimer();
+            if (volumeType == VolumeType::ColorTSDF)
+                volume->raycast(pose, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors);
+            else
+                volume->raycast(pose, frameSize.height, frameSize.width, intrRaycast, upoints, unormals);
+            stopTimer();
+
+            done = true;
+        }
+
+        if (display)
+        {
+            Mat points, normals, colors;
+            points = upoints.getMat(ACCESS_READ);
+            normals = unormals.getMat(ACCESS_READ);
+            colors = ucolors.getMat(ACCESS_READ);
+
+            Vec3f lightPose = Vec3f::all(0.f);
+            if (volumeType == VolumeType::ColorTSDF)
+                displayColorImage(depth, rgb, points, normals, colors, depthFactor, lightPose);
+            else
+                displayImage(depth, points, normals, depthFactor, lightPose);
+        }
+    }
+    SANITY_CHECK_NOTHING();
+}
+
+//TODO: fix it when ColorTSDF gets GPU version
+INSTANTIATE_TEST_CASE_P(Volume, VolumePerfFixture, /*::testing::Combine(PlatformTypeEnum::all(), VolumeTypeEnum::all())*/
+                        ::testing::Combine(
+                        ::testing::Values(PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
+                                          PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF},
+                                          PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
+                                          PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF},
+                                          PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF}),
+                        VolumeTestSrcTypeEnum::all(), SequenceEnum::all()));
+
+}} // namespace

modules/ptcloud/samples/odometry_evaluation.cpp

@@ -0,0 +1,225 @@
+// 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
+
+#include <opencv2/ptcloud.hpp>
+
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/core/quaternion.hpp>
+
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace cv;
+
+#define BILATERAL_FILTER 0// if 1 then bilateral filter will be used for the depth
+
+static
+void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
+{
+    CV_Assert( timestamps.size() == Rt.size() );
+
+    ofstream file( filename.c_str() );
+    if( !file.is_open() )
+        return;
+
+    cout.precision(4);
+    for( size_t i = 0; i < Rt.size(); i++ )
+    {
+        const Mat& Rt_curr = Rt[i];
+        if( Rt_curr.empty() )
+            continue;
+
+        CV_Assert( Rt_curr.type() == CV_64FC1 );
+
+        Quatd rot = Quatd::createFromRotMat(Rt_curr(Rect(0, 0, 3, 3)));
+
+        // timestamp tx ty tz qx qy qz qw
+        file << timestamps[i] << " " << fixed
+             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
+             << rot.x << " " << rot.y << " " << rot.z << " " << rot.w << endl;
+
+    }
+    file.close();
+}
+
+static
+void setCameraMatrixFreiburg1(float& fx, float& fy, float& cx, float& cy)
+{
+    fx = 517.3f; fy = 516.5f; cx = 318.6f; cy = 255.3f;
+}
+
+static
+void setCameraMatrixFreiburg2(float& fx, float& fy, float& cx, float& cy)
+{
+    fx = 520.9f; fy = 521.0f; cx = 325.1f; cy = 249.7f;
+}
+
+/*
+ * This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
+ * At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory
+ * to file of format that the benchmark requires. Saved file can be used for online evaluation.
+ */
+int main(int argc, char** argv)
+{
+    if(argc != 4)
+    {
+        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
+        return -1;
+    }
+
+    vector<string> timestamps;
+    vector<Mat> Rts;
+
+    const string filename = argv[1];
+    ifstream file( filename.c_str() );
+    if( !file.is_open() )
+        return -1;
+
+    char dlmrt1 = '/';
+    char dlmrt2 = '\\';
+
+    size_t pos1 = filename.rfind(dlmrt1);
+    size_t pos2 = filename.rfind(dlmrt2);
+    size_t pos = pos1 < pos2 ? pos1 : pos2;
+    char dlmrt = pos1 < pos2 ? dlmrt1 : dlmrt2;
+
+    string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;
+
+    const int timestampLength = 17;
+    const int rgbPathLehgth = 17+8;
+    const int depthPathLehgth = 17+10;
+
+    float fx = 525.0f, // default
+          fy = 525.0f,
+          cx = 319.5f,
+          cy = 239.5f;
+    if(filename.find("freiburg1") != string::npos)
+        setCameraMatrixFreiburg1(fx, fy, cx, cy);
+    if(filename.find("freiburg2") != string::npos)
+        setCameraMatrixFreiburg2(fx, fy, cx, cy);
+    Mat cameraMatrix = Mat::eye(3,3,CV_32FC1);
+    {
+        cameraMatrix.at<float>(0,0) = fx;
+        cameraMatrix.at<float>(1,1) = fy;
+        cameraMatrix.at<float>(0,2) = cx;
+        cameraMatrix.at<float>(1,2) = cy;
+    }
+
+    OdometrySettings ods;
+    ods.setCameraMatrix(cameraMatrix);
+    Odometry odometry;
+    String odname = string(argv[3]);
+    if (odname == "Rgbd")
+        odometry = Odometry(OdometryType::RGB, ods, OdometryAlgoType::COMMON);
+    else if (odname == "ICP")
+        odometry = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::COMMON);
+    else if (odname == "RgbdICP")
+        odometry = Odometry(OdometryType::RGB_DEPTH, ods, OdometryAlgoType::COMMON);
+    else if (odname == "FastICP")
+        odometry = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
+    else
+    {
+        std::cout << "Can not create Odometry algorithm. Check the passed odometry name." << std::endl;
+        return -1;
+    }
+
+    OdometryFrame frame_prev, frame_curr;
+
+    TickMeter gtm;
+    int count = 0;
+    for(int i = 0; !file.eof(); i++)
+    {
+        string str;
+        std::getline(file, str);
+        if(str.empty()) break;
+        if(str.at(0) == '#') continue; /* comment */
+
+        Mat image, depth;
+        // Read one pair (rgb and depth)
+        // example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
+#if BILATERAL_FILTER
+        TickMeter tm_bilateral_filter;
+#endif
+        {
+            string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
+            string timestap = str.substr(0, timestampLength);
+            string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
+
+            image = imread(dirname + rgbFilename);
+            depth = imread(dirname + depthFilename, -1);
+
+            CV_Assert(!image.empty());
+            CV_Assert(!depth.empty());
+            CV_Assert(depth.type() == CV_16UC1);
+
+            // scale depth
+            Mat depth_flt;
+            depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
+#if !BILATERAL_FILTER
+            depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
+            depth = depth_flt;
+#else
+            tm_bilateral_filter.start();
+            depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0));
+            const double depth_sigma = 0.03;
+            const double space_sigma = 4.5;  // in pixels
+            Mat invalidDepthMask = depth_flt == 0.f;
+            depth_flt.setTo(-5*depth_sigma, invalidDepthMask);
+            bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma);
+            depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask);
+            tm_bilateral_filter.stop();
+            cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl;
+#endif
+            timestamps.push_back( timestap );
+        }
+
+        {
+            Mat gray;
+            cvtColor(image, gray, COLOR_BGR2GRAY);
+            frame_curr = OdometryFrame(depth, gray);
+
+            Mat Rt;
+            if(!Rts.empty())
+            {
+                TickMeter tm;
+                tm.start();
+                gtm.start();
+                odometry.prepareFrames(frame_curr, frame_prev);
+                bool res = odometry.compute(frame_curr, frame_prev, Rt);
+                gtm.stop();
+                tm.stop();
+                count++;
+                cout << "Time " << tm.getTimeSec() << endl;
+#if BILATERAL_FILTER
+                cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl;
+#endif
+                if(!res)
+                    Rt = Mat::eye(4,4,CV_64FC1);
+            }
+
+            if( Rts.empty() )
+                Rts.push_back(Mat::eye(4,4,CV_64FC1));
+            else
+            {
+                Mat& prevRt = *Rts.rbegin();
+                cout << "Rt " << Rt << endl;
+                Rts.push_back( prevRt * Rt );
+            }
+
+            //if (!frame_prev.empty())
+            //    frame_prev.release();
+            frame_prev = frame_curr;
+            frame_curr = OdometryFrame();
+            //std::swap(frame_prev, frame_curr);
+        }
+    }
+
+    std::cout << "Average time " << gtm.getAvgTimeSec() << std::endl;
+    writeResults(argv[2], timestamps, Rts);
+
+    return 0;
+}

modules/ptcloud/src/color_tsdf_functions.hpp

@@ -0,0 +1,36 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+#ifndef OPENCV_3D_COLORED_TSDF_FUNCTIONS_HPP
+#define OPENCV_3D_COLORED_TSDF_FUNCTIONS_HPP
+
+#include <unordered_set>
+
+#include "utils.hpp"
+#include "tsdf_functions.hpp"
+
+#define USE_INTERPOLATION_IN_GETNORMAL 1
+
+namespace cv
+{
+void integrateColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose,
+                                  InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume);
+void integrateColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &volumePose, const Matx44f &cameraPose,
+                                  InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume);
+void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose,
+                                int height, int width, InputArray intr,
+                                InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors);
+void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume,
+                                         InputArray _points, OutputArray _normals);
+void fetchPointsNormalsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume,
+                                               OutputArray _points, OutputArray _normals);
+void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume,
+                                                     OutputArray _points, OutputArray _normals, OutputArray _colors);
+
+} // namespace cv
+
+#endif

modules/ptcloud/src/depth_registration.cpp

@@ -0,0 +1,315 @@
+// 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
+
+#include "precomp.hpp"
+
+namespace cv
+{
+
+///////////////////////////////////////////////////////////////////////////////////
+
+// Our three input types have a different value for a depth pixel with no depth
+template<typename DepthDepth>
+inline DepthDepth noDepthSentinelValue()
+{
+    return 0;
+}
+
+template<>
+inline float
+noDepthSentinelValue<float>()
+{
+    return std::numeric_limits<float>::quiet_NaN();
+}
+
+template<>
+inline double
+noDepthSentinelValue<double>()
+{
+    return std::numeric_limits<double>::quiet_NaN();
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+
+// Testing for depth pixels with no depth isn't straightforward for NaN values. We
+// need to specialize the equality check for floats and doubles.
+template<typename DepthDepth>
+inline bool
+isEqualToNoDepthSentinelValue(const DepthDepth& value)
+{
+    return value == noDepthSentinelValue<DepthDepth>();
+}
+
+template<>
+inline bool
+isEqualToNoDepthSentinelValue<float>(const float& value)
+{
+    return cvIsNaN(value) != 0;
+}
+
+template<>
+inline bool
+isEqualToNoDepthSentinelValue<double>(const double& value)
+{
+    return cvIsNaN(value) != 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+
+
+// When using the unsigned short representation, we'd like to round the values to the nearest
+// integer value. The float/double representations don't need to be rounded
+template<typename DepthDepth>
+inline DepthDepth
+floatToInputDepth(const float& value)
+{
+    return (DepthDepth)value;
+}
+
+template<>
+inline unsigned short
+floatToInputDepth<unsigned short>(const float& value)
+{
+    return (unsigned short)(value + 0.5);
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+
+
+/** Computes a registered depth image from an unregistered image.
+ *
+ * @param unregisteredDepth the input depth data
+ * @param unregisteredCameraMatrix the camera matrix of the depth camera
+ * @param registeredCameraMatrix the camera matrix of the external camera
+ * @param registeredDistCoeffs the distortion coefficients of the external camera
+ * @param rbtRgb2Depth the rigid body transform between the cameras.
+ * @param outputImagePlaneSize the image plane dimensions of the external camera (width, height)
+ * @param depthDilation whether or not the depth is dilated to avoid holes and occlusion errors
+ * @param inputDepthToMetersScale the scale needed to transform the input depth units to meters
+ * @param registeredDepth the result of transforming the depth into the external camera
+ */
+template<typename DepthDepth>
+void performRegistration(const Mat_<DepthDepth>& unregisteredDepth,
+                         const Matx33f& unregisteredCameraMatrix,
+                         const Matx33f& registeredCameraMatrix,
+                         const Mat_<float>& registeredDistCoeffs,
+                         const Matx44f& rbtRgb2Depth,
+                         const Size outputImagePlaneSize,
+                         const bool depthDilation,
+                         const float inputDepthToMetersScale,
+                         Mat& registeredDepth)
+{
+    // Create output Mat of the correct type, filled with an initial value indicating no depth
+    registeredDepth = Mat_<DepthDepth>(outputImagePlaneSize, noDepthSentinelValue<DepthDepth>());
+
+    // Figure out whether we'll have to apply a distortion
+    bool hasDistortion = (countNonZero(registeredDistCoeffs) > 0);
+
+    // A point (i,j,1) will have to be converted to 3d first, by multiplying it by K.inv()
+    // It will then be transformed by rbtRgb2Depth.
+    // Finally, it will be projected into the external camera via registeredCameraMatrix and
+    // its distortion coefficients. If there is no distortion in the external camera, we
+    // can linearly chain all three operations together.
+
+    Matx44f K = Matx44f::zeros();
+    for (unsigned char j = 0; j < 3; ++j)
+        for (unsigned char i = 0; i < 3; ++i)
+        {
+            K(j, i) = unregisteredCameraMatrix(j, i);
+        }
+    K(3, 3) = 1;
+
+    Matx44f initialProjection;
+    if (hasDistortion)
+    {
+        // The projection into the external camera will be done separately with distortion
+        initialProjection = rbtRgb2Depth * K.inv();
+    }
+    else
+    {
+        // No distortion, so all operations can be chained
+        initialProjection = Matx44f::zeros();
+        for (unsigned char j = 0; j < 3; ++j)
+            for (unsigned char i = 0; i < 3; ++i)
+                initialProjection(j, i) = registeredCameraMatrix(j, i);
+        initialProjection(3, 3) = 1;
+
+        initialProjection = initialProjection * rbtRgb2Depth * K.inv();
+    }
+
+    // Apply the initial projection to the input depth
+    Mat_<Point3f> transformedCloud;
+    {
+        Mat_<Point3f> point_tmp(outputImagePlaneSize, Point3f(0., 0., 0.));
+        for (int j = 0; j < unregisteredDepth.rows; ++j)
+        {
+            const DepthDepth* unregisteredDepthPtr = unregisteredDepth[j];
+
+            Point3f* point = point_tmp[j];
+            for (int i = 0; i < unregisteredDepth.cols; ++i, ++unregisteredDepthPtr, ++point)
+            {
+                float rescaled_depth = float(*unregisteredDepthPtr) * inputDepthToMetersScale;
+
+                // If the DepthDepth is of type unsigned short, zero is a sentinel value to indicate
+                // no depth. CV_32F and CV_64F should already have NaN for no depth values.
+                if (rescaled_depth == 0)
+                {
+                    rescaled_depth = std::numeric_limits<float>::quiet_NaN();
+                }
+
+                point->x = i * rescaled_depth;
+                point->y = j * rescaled_depth;
+                point->z = rescaled_depth;
+            }
+        }
+
+        perspectiveTransform(point_tmp, transformedCloud, initialProjection);
+    }
+
+    std::vector<Point2f> transformedAndProjectedPoints(transformedCloud.cols);
+    const float metersToInputUnitsScale = 1 / inputDepthToMetersScale;
+    const Rect registeredDepthBounds(Point(), outputImagePlaneSize);
+
+    for (int y = 0; y < transformedCloud.rows; y++)
+    {
+        if (hasDistortion)
+        {
+
+            // Project an entire row of points with distortion.
+            // Doing this for the entire image at once would require more memory.
+            projectPoints(transformedCloud.row(y),
+                          Vec3f(0, 0, 0),
+                          Vec3f(0, 0, 0),
+                          registeredCameraMatrix,
+                          registeredDistCoeffs,
+                          transformedAndProjectedPoints);
+        }
+        else
+        {
+            // With no distortion, we just have to dehomogenize the point since all major transforms
+            // already happened with initialProjection.
+            Point2f* point2d = &transformedAndProjectedPoints[0];
+            const Point2f* point2d_end = point2d + transformedAndProjectedPoints.size();
+            const Point3f* point3d = transformedCloud[y];
+            for (; point2d < point2d_end; ++point2d, ++point3d)
+            {
+                point2d->x = point3d->x / point3d->z;
+                point2d->y = point3d->y / point3d->z;
+            }
+        }
+
+        const Point2f* outputProjectedPoint = &transformedAndProjectedPoints[0];
+        const Point3f* p = transformedCloud[y], * p_end = p + transformedCloud.cols;
+
+        for (; p < p_end; ++outputProjectedPoint, ++p)
+        {
+            // Skip this one if there isn't a valid depth
+            const Point2f projectedPixelFloatLocation = *outputProjectedPoint;
+            if (cvIsNaN(projectedPixelFloatLocation.x))
+                continue;
+
+            //Get integer pixel location
+            const Point2i projectedPixelLocation = projectedPixelFloatLocation;
+
+            // Ensure that the projected point is actually contained in our output image
+            if (!registeredDepthBounds.contains(projectedPixelLocation))
+                continue;
+
+            // Go back to our original scale, since that's what our output will be
+            // The templated function is to ensure that integer values are rounded to the nearest integer
+            const DepthDepth cloudDepth = floatToInputDepth<DepthDepth>(p->z * metersToInputUnitsScale);
+
+            DepthDepth& outputDepth = registeredDepth.at<DepthDepth>(projectedPixelLocation.y, projectedPixelLocation.x);
+
+            // Occlusion check
+            if (isEqualToNoDepthSentinelValue<DepthDepth>(outputDepth) || (outputDepth > cloudDepth))
+                outputDepth = cloudDepth;
+
+            // If desired, dilate this point to avoid holes in the final image
+            if (depthDilation)
+            {
+                // Choosing to dilate in a 2x2 region, where the original projected location is in the bottom right of this
+                // region. This is what's done on PrimeSense devices, but a more accurate scheme could be used.
+                const Point2i dilatedProjectedLocations[3] = { Point2i(projectedPixelLocation.x - 1, projectedPixelLocation.y),
+                                                               Point2i(projectedPixelLocation.x    , projectedPixelLocation.y - 1),
+                                                               Point2i(projectedPixelLocation.x - 1, projectedPixelLocation.y - 1) };
+
+                for (int i = 0; i < 3; i++)
+                {
+                    const Point2i& dilatedCoordinates = dilatedProjectedLocations[i];
+
+                    if (!registeredDepthBounds.contains(dilatedCoordinates))
+                        continue;
+
+                    DepthDepth& outputDilatedDepth = registeredDepth.at<DepthDepth>(dilatedCoordinates.y, dilatedCoordinates.x);
+
+                    // Occlusion check
+                    if (isEqualToNoDepthSentinelValue(outputDilatedDepth) || (outputDilatedDepth > cloudDepth))
+                        outputDilatedDepth = cloudDepth;
+                }
+
+            } // depthDilation
+
+        } // iterate cols
+    } // iterate rows
+}
+
+
+
+void
+registerDepth(InputArray unregisteredCameraMatrix, InputArray registeredCameraMatrix, InputArray registeredDistCoeffs,
+              InputArray Rt, InputArray unregisteredDepth, const Size& outputImagePlaneSize,
+              OutputArray registeredDepth, bool depthDilation)
+{
+    CV_Assert(unregisteredCameraMatrix.depth() == CV_64F || unregisteredCameraMatrix.depth() == CV_32F);
+    CV_Assert(registeredCameraMatrix.depth() == CV_64F   || registeredCameraMatrix.depth() == CV_32F);
+    CV_Assert(registeredDistCoeffs.empty() || registeredDistCoeffs.depth() == CV_64F || registeredDistCoeffs.depth() == CV_32F);
+    CV_Assert(Rt.depth() == CV_64F || Rt.depth() == CV_32F);
+
+    CV_Assert(unregisteredDepth.cols() > 0 && unregisteredDepth.rows() > 0 &&
+              (unregisteredDepth.depth() == CV_32F || unregisteredDepth.depth() == CV_64F || unregisteredDepth.depth() == CV_16U));
+    CV_Assert(outputImagePlaneSize.height > 0 && outputImagePlaneSize.width > 0);
+
+    // Implicitly checking dimensions of the InputArrays
+    Matx33f _unregisteredCameraMatrix = unregisteredCameraMatrix.getMat();
+    Matx33f _registeredCameraMatrix = registeredCameraMatrix.getMat();
+    Mat_<float> _registeredDistCoeffs = registeredDistCoeffs.getMat();
+    Matx44f _rbtRgb2Depth = Rt.getMat();
+
+    Mat& registeredDepthMat = registeredDepth.getMatRef();
+
+    switch (unregisteredDepth.depth())
+    {
+    case CV_16U:
+    {
+        performRegistration<unsigned short>(unregisteredDepth.getMat(), _unregisteredCameraMatrix,
+            _registeredCameraMatrix, _registeredDistCoeffs,
+            _rbtRgb2Depth, outputImagePlaneSize, depthDilation,
+            .001f, registeredDepthMat);
+        break;
+    }
+    case CV_32F:
+    {
+        performRegistration<float>(unregisteredDepth.getMat(), _unregisteredCameraMatrix,
+                                   _registeredCameraMatrix, _registeredDistCoeffs,
+                                   _rbtRgb2Depth, outputImagePlaneSize, depthDilation,
+                                   1.0f, registeredDepthMat);
+        break;
+    }
+    case CV_64F:
+    {
+        performRegistration<double>(unregisteredDepth.getMat(), _unregisteredCameraMatrix,
+                                    _registeredCameraMatrix, _registeredDistCoeffs,
+                                    _rbtRgb2Depth, outputImagePlaneSize, depthDilation,
+                                    1.0f, registeredDepthMat);
+        break;
+    }
+    default:
+    {
+        CV_Error(Error::StsUnsupportedFormat, "Input depth must be unsigned short, float, or double.");
+    }
+    }
+}
+
+} /* namespace cv */

modules/ptcloud/src/depth_to_3d.cpp

@@ -0,0 +1,254 @@
+// 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
+
+#include "precomp.hpp"
+#include "depth_to_3d.hpp"
+
+namespace cv
+{
+
+/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+ * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+ * Otherwise, the image is simply converted to floats
+ * @param in_in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), it is assumed in meters)
+ * @param depth the desired output depth (floats or double)
+ * @param out_out The rescaled float depth image
+ */
+void rescaleDepth(InputArray in_in, int type, OutputArray out_out, double depth_factor)
+{
+    cv::Mat in = in_in.getMat();
+    CV_Assert(in.type() == CV_64FC1 || in.type() == CV_32FC1 || in.type() == CV_16UC1 || in.type() == CV_16SC1);
+    CV_Assert(type == CV_64FC1 || type == CV_32FC1);
+
+    int in_depth = in.depth();
+
+    out_out.create(in.size(), type);
+    cv::Mat out = out_out.getMat();
+    if (in_depth == CV_16U)
+    {
+      in.convertTo(out, type, 1 / depth_factor); //convert to float so that it is in meters
+      cv::Mat valid_mask = in == std::numeric_limits<ushort>::min(); // Should we do std::numeric_limits<ushort>::max() too ?
+      out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
+    }
+    if (in_depth == CV_16S)
+    {
+      in.convertTo(out, type, 1 / depth_factor); //convert to float so tha$
+      cv::Mat valid_mask = (in == std::numeric_limits<short>::min()) | (in == std::numeric_limits<short>::max()); // Should we do std::numeric_limits<ushort>::max() too ?
+      out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
+    }
+    if ((in_depth == CV_32F) || (in_depth == CV_64F))
+      in.convertTo(out, type);
+}
+
+
+/**
+ * @param K
+ * @param depth the depth image
+ * @param mask the mask of the points to consider (can be empty)
+ * @param points3d the resulting 3d points, a 3-channel matrix
+ */
+static void depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const cv::Mat& v_mat, const cv::Mat& z_mat,
+                               cv::Mat& points3d)
+{
+    CV_Assert((u_mat.size() == z_mat.size()) && (v_mat.size() == z_mat.size()));
+    if (u_mat.empty())
+        return;
+    CV_Assert((u_mat.type() == z_mat.type()) && (v_mat.type() == z_mat.type()));
+
+    //grab camera params
+    cv::Mat_<float> K;
+
+    if (in_K.depth() == CV_32F)
+        K = in_K;
+    else
+        in_K.convertTo(K, CV_32F);
+
+    float fx = K(0, 0);
+    float fy = K(1, 1);
+    float s = K(0, 1);
+    float cx = K(0, 2);
+    float cy = K(1, 2);
+
+    std::vector<cv::Mat> coordinates(4);
+
+    coordinates[0] = (u_mat - cx) / fx;
+
+    if (s != 0)
+        coordinates[0] = coordinates[0] + (-(s / fy) * v_mat + cy * s / fy) / fx;
+
+    coordinates[0] = coordinates[0].mul(z_mat);
+    coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy);
+    coordinates[2] = z_mat;
+    coordinates[3] = Mat(u_mat.size(), CV_32F, Scalar(0));
+    cv::merge(coordinates, points3d);
+}
+
+/**
+ * @param K
+ * @param depth the depth image
+ * @param mask the mask of the points to consider (can be empty)
+ * @param points3d the resulting 3d points
+ */
+static void depthTo3dMask(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& mask, cv::Mat& points3d)
+{
+    // Create 3D points in one go.
+    cv::Mat_<float> u_mat, v_mat, z_mat;
+
+    cv::Mat_<uchar> uchar_mask = mask;
+
+    if (mask.depth() != (CV_8U))
+        mask.convertTo(uchar_mask, CV_8U);
+
+    // Figure out the interesting indices
+    size_t n_points;
+
+    if (depth.depth() == CV_16U)
+        n_points = convertDepthToFloat<ushort>(depth, mask, 1.0f / 1000.0f, u_mat, v_mat, z_mat);
+    else if (depth.depth() == CV_16S)
+        n_points = convertDepthToFloat<short>(depth, mask, 1.0f / 1000.0f, u_mat, v_mat, z_mat);
+    else
+    {
+        CV_Assert(depth.type() == CV_32F);
+        n_points = convertDepthToFloat<float>(depth, mask, 1.0f, u_mat, v_mat, z_mat);
+    }
+
+    if (n_points == 0)
+        return;
+
+    u_mat.resize(n_points);
+    v_mat.resize(n_points);
+    z_mat.resize(n_points);
+
+    depthTo3d_from_uvz(K, u_mat, v_mat, z_mat, points3d);
+    points3d = points3d.reshape(4, 1);
+}
+
+/**
+ * @param K
+ * @param depth the depth image
+ * @param points3d the resulting 3d points
+ */
+template<typename T>
+void depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_<T>& K, cv::Mat& points3d)
+{
+    const T inv_fx = T(1) / K(0, 0);
+    const T inv_fy = T(1) / K(1, 1);
+    const T ox = K(0, 2);
+    const T oy = K(1, 2);
+
+    // Build z
+    cv::Mat_<T> z_mat;
+    if (z_mat.depth() == in_depth.depth())
+        z_mat = in_depth;
+    else
+        rescaleDepthTemplated<T>(in_depth, z_mat);
+
+    // Pre-compute some constants
+    cv::Mat_<T> x_cache(1, in_depth.cols), y_cache(in_depth.rows, 1);
+    T* x_cache_ptr = x_cache[0], * y_cache_ptr = y_cache[0];
+    for (int x = 0; x < in_depth.cols; ++x, ++x_cache_ptr)
+        *x_cache_ptr = (x - ox) * inv_fx;
+    for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
+        *y_cache_ptr = (y - oy) * inv_fy;
+
+    y_cache_ptr = y_cache[0];
+    for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
+    {
+        cv::Vec<T, 4>* point = points3d.ptr<cv::Vec<T, 4> >(y);
+        const T* x_cache_ptr_end = x_cache[0] + in_depth.cols;
+        const T* depth = z_mat[y];
+        for (x_cache_ptr = x_cache[0]; x_cache_ptr != x_cache_ptr_end; ++x_cache_ptr, ++point, ++depth)
+        {
+            T z = *depth;
+            (*point)[0] = (*x_cache_ptr) * z;
+            (*point)[1] = (*y_cache_ptr) * z;
+            (*point)[2] = z;
+            (*point)[3] = 0;
+        }
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+/**
+ * @param K
+ * @param depth the depth image
+ * @param u_mat the list of x coordinates
+ * @param v_mat the list of matching y coordinates
+ * @param points3d the resulting 3d points
+ */
+void depthTo3dSparse(InputArray depth_in, InputArray K_in, InputArray points_in, OutputArray points3d_out)
+{
+    // Make sure we use foat types
+    cv::Mat points = points_in.getMat();
+    cv::Mat depth = depth_in.getMat();
+
+    cv::Mat points_float;
+    if (points.depth() != CV_32F)
+        points.convertTo(points_float, CV_32FC2);
+    else
+        points_float = points;
+
+    // Fill the depth matrix
+    cv::Mat_<float> z_mat;
+
+    if (depth.depth() == CV_16U)
+        convertDepthToFloat<ushort>(depth, 1.0f / 1000.0f, points_float, z_mat);
+    else if (depth.depth() == CV_16U)
+        convertDepthToFloat<short>(depth, 1.0f / 1000.0f, points_float, z_mat);
+    else
+    {
+        CV_Assert(depth.type() == CV_32F);
+        convertDepthToFloat<float>(depth, 1.0f, points_float, z_mat);
+    }
+
+    std::vector<cv::Mat> channels(2);
+    cv::split(points_float, channels);
+
+    points3d_out.create(channels[0].rows, channels[0].cols, CV_32FC4);
+    cv::Mat points3d = points3d_out.getMat();
+    depthTo3d_from_uvz(K_in.getMat(), channels[0], channels[1], z_mat, points3d);
+}
+
+/**
+ * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), otherwise, if given as CV_32F, it is assumed in meters)
+ * @param K The calibration matrix
+ * @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the
+ *        depth of `K` if `depth` is of depth CV_U
+ * @param mask the mask of the points to consider (can be empty)
+ */
+void depthTo3d(InputArray depth_in, InputArray K_in, OutputArray points3d_out, InputArray mask_in)
+{
+    cv::Mat depth = depth_in.getMat();
+    cv::Mat K = K_in.getMat();
+    cv::Mat mask = mask_in.getMat();
+    CV_Assert(K.cols == 3 && K.rows == 3 && (K.depth() == CV_64F || K.depth() == CV_32F));
+    CV_Assert(depth.type() == CV_64FC1 || depth.type() == CV_32FC1 || depth.type() == CV_16UC1 || depth.type() == CV_16SC1);
+    CV_Assert(mask.empty() || mask.channels() == 1);
+
+    cv::Mat K_new;
+    K.convertTo(K_new, depth.depth() == CV_64F ? CV_64F : CV_32F); // issue #1021
+
+    // Create 3D points in one go.
+    if (!mask.empty())
+    {
+        cv::Mat points3d;
+        depthTo3dMask(depth, K_new, mask, points3d);
+        points3d_out.create(points3d.size(), CV_MAKETYPE(K_new.depth(), 4));
+        points3d.copyTo(points3d_out.getMat());
+    }
+    else
+    {
+        points3d_out.create(depth.size(), CV_MAKETYPE(K_new.depth(), 4));
+        cv::Mat points3d = points3d_out.getMat();
+        if (K_new.depth() == CV_64F)
+            depthTo3dNoMask<double>(depth, K_new, points3d);
+        else
+            depthTo3dNoMask<float>(depth, K_new, points3d);
+    }
+}
+
+}

modules/ptcloud/src/depth_to_3d.hpp

@@ -0,0 +1,115 @@
+// 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_DEPTH_TO_3D_HPP
+#define OPENCV_3D_DEPTH_TO_3D_HPP
+
+#include "precomp.hpp"
+#include "utils.hpp"
+
+namespace cv
+{
+
+/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+ * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+ * Otherwise, the image is simply converted to floats
+ * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), it is assumed in meters)
+ * @param the desired output depth (floats or double)
+ * @param out The rescaled float depth image
+ */
+/* void rescaleDepth(InputArray in_in, int depth, OutputArray out_out); */
+
+template<typename T>
+void
+rescaleDepthTemplated(const Mat& in, Mat& out);
+
+template<>
+inline void
+rescaleDepthTemplated<float>(const Mat& in, Mat& out)
+{
+  rescaleDepth(in, CV_32F, out);
+}
+
+template<>
+inline void
+rescaleDepthTemplated<double>(const Mat& in, Mat& out)
+{
+  rescaleDepth(in, CV_64F, out);
+}
+
+/**
+ * @param depth the depth image, containing depth with the value T
+ * @param the mask, containing CV_8UC1
+ */
+template <typename T>
+size_t convertDepthToFloat(const cv::Mat& depth, const cv::Mat& mask, float scale, cv::Mat_<float> &u_mat, cv::Mat_<float> &v_mat, cv::Mat_<float> &z_mat)
+{
+    CV_Assert(depth.size == mask.size);
+
+    cv::Size depth_size = depth.size();
+
+    cv::Mat_<uchar> uchar_mask = mask;
+
+    if ((mask.depth() != CV_8S) && (mask.depth() != CV_8U) && (mask.depth() != CV_Bool))
+        mask.convertTo(uchar_mask, CV_8U);
+
+    u_mat = cv::Mat_<float>(depth_size.area(), 1);
+    v_mat = cv::Mat_<float>(depth_size.area(), 1);
+    z_mat = cv::Mat_<float>(depth_size.area(), 1);
+
+    // Raw data from the Kinect has int
+    size_t n_points = 0;
+
+    for (int v = 0; v < depth_size.height; v++)
+    {
+        uchar* r = uchar_mask.ptr<uchar>(v, 0);
+
+        for (int u = 0; u < depth_size.width; u++, ++r)
+            if (*r)
+            {
+                u_mat((int)n_points, 0) = (float)u;
+                v_mat((int)n_points, 0) = (float)v;
+                T depth_i = depth.at<T>(v, u);
+
+                if (cvIsNaN((float)depth_i) || (depth_i == std::numeric_limits<T>::min()) || (depth_i == std::numeric_limits<T>::max()))
+                    z_mat((int)n_points, 0) = std::numeric_limits<float>::quiet_NaN();
+                else
+                    z_mat((int)n_points, 0) = depth_i * scale;
+
+                ++n_points;
+            }
+    }
+
+    return n_points;
+}
+
+/**
+ * @param depth the depth image, containing depth with the value T
+ * @param the mask, containing CV_8UC1
+ */
+template <typename T>
+void convertDepthToFloat(const cv::Mat& depth, float scale, const cv::Mat &uv_mat, cv::Mat_<float> &z_mat)
+{
+    z_mat = cv::Mat_<float>(uv_mat.size());
+
+    // Raw data from the Kinect has int
+    float* z_mat_iter = reinterpret_cast<float*>(z_mat.data);
+
+    for (cv::Mat_<cv::Vec2f>::const_iterator uv_iter = uv_mat.begin<cv::Vec2f>(), uv_end = uv_mat.end<cv::Vec2f>();
+        uv_iter != uv_end; ++uv_iter, ++z_mat_iter)
+    {
+        T depth_i = depth.at < T >((int)(*uv_iter)[1], (int)(*uv_iter)[0]);
+
+        if (cvIsNaN((float)depth_i) || (depth_i == std::numeric_limits < T > ::min())
+            || (depth_i == std::numeric_limits < T > ::max()))
+            *z_mat_iter = std::numeric_limits<float>::quiet_NaN();
+        else
+            *z_mat_iter = depth_i * scale;
+    }
+}
+
+}
+
+#endif // include guard

modules/ptcloud/src/hash_tsdf_functions.hpp

@@ -0,0 +1,324 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+#ifndef OPENCV_3D_HASH_TSDF_FUNCTIONS_HPP
+#define OPENCV_3D_HASH_TSDF_FUNCTIONS_HPP
+
+#include <unordered_set>
+
+#include "utils.hpp"
+#include "tsdf_functions.hpp"
+
+#define USE_INTERPOLATION_IN_GETNORMAL 1
+#define VOLUMES_SIZE 8192
+
+namespace cv
+{
+
+//! 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;
+    }
+};
+
+struct VolumeUnit
+{
+    cv::Vec3i coord;
+    int index;
+    cv::Matx44f pose;
+    int lastVisibleIndex = 0;
+    bool isActive;
+};
+
+class CustomHashSet
+{
+public:
+    static const int hashDivisor = 32768;
+    static const int startCapacity = 2048;
+
+    std::vector<int> hashes;
+    // 0-3 for key, 4th for internal use
+    // don't keep keep value
+    std::vector<Vec4i> data;
+    int capacity;
+    int last;
+
+    CustomHashSet()
+    {
+        hashes.resize(hashDivisor);
+        for (int i = 0; i < hashDivisor; i++)
+            hashes[i] = -1;
+        capacity = startCapacity;
+
+        data.resize(capacity);
+        for (int i = 0; i < capacity; i++)
+            data[i] = { 0, 0, 0, -1 };
+
+        last = 0;
+    }
+
+    ~CustomHashSet() { }
+
+    inline size_t calc_hash(Vec3i x) const
+    {
+        uint32_t seed = 0;
+        constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
+        for (int i = 0; i < 3; i++)
+        {
+            seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+        }
+        return seed;
+    }
+
+    // should work on existing elements too
+    // 0 - need resize
+    // 1 - idx is inserted
+    // 2 - idx already exists
+    int insert(Vec3i idx)
+    {
+        if (last < capacity)
+        {
+            int hash = int(calc_hash(idx) % hashDivisor);
+            int place = hashes[hash];
+            if (place >= 0)
+            {
+                int oldPlace = place;
+                while (place >= 0)
+                {
+                    if (data[place][0] == idx[0] &&
+                        data[place][1] == idx[1] &&
+                        data[place][2] == idx[2])
+                        return 2;
+                    else
+                    {
+                        oldPlace = place;
+                        place = data[place][3];
+                        //std::cout << "place=" << place << std::endl;
+                    }
+                }
+
+                // found, create here
+                data[oldPlace][3] = last;
+            }
+            else
+            {
+                // insert at last
+                hashes[hash] = last;
+            }
+
+            data[last][0] = idx[0];
+            data[last][1] = idx[1];
+            data[last][2] = idx[2];
+            data[last][3] = -1;
+            last++;
+
+            return 1;
+        }
+        else
+            return 0;
+    }
+
+    int find(Vec3i idx) const
+    {
+        int hash = int(calc_hash(idx) % hashDivisor);
+        int place = hashes[hash];
+        // search a place
+        while (place >= 0)
+        {
+            if (data[place][0] == idx[0] &&
+                data[place][1] == idx[1] &&
+                data[place][2] == idx[2])
+                break;
+            else
+            {
+                place = data[place][3];
+            }
+        }
+
+        return place;
+    }
+};
+
+// TODO: remove this structure as soon as HashTSDFGPU data is completely on GPU;
+// until then CustomHashTable can be replaced by this one if needed
+
+const int NAN_ELEMENT = -2147483647;
+
+struct Volume_NODE
+{
+    Vec4i idx = Vec4i(NAN_ELEMENT);
+    int32_t row = -1;
+    int32_t nextVolumeRow = -1;
+    int32_t dummy = 0;
+    int32_t dummy2 = 0;
+};
+
+const int _hash_divisor = 32768;
+const int _list_size = 4;
+
+class VolumesTable
+{
+public:
+    const int hash_divisor = _hash_divisor;
+    const int list_size = _list_size;
+    const int32_t free_row = -1;
+    const int32_t free_isActive = 0;
+
+    const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT);
+
+    int bufferNums;
+    cv::Mat volumes;
+
+    VolumesTable() : bufferNums(1)
+    {
+        this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType<Volume_NODE>());
+        for (int i = 0; i < volumes.size().height; i++)
+        {
+            Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
+            v->idx = nan4;
+            v->row = -1;
+            v->nextVolumeRow = -1;
+        }
+    }
+    const VolumesTable& operator=(const VolumesTable& vt)
+    {
+        this->volumes = vt.volumes;
+        this->bufferNums = vt.bufferNums;
+        return *this;
+    }
+    ~VolumesTable() {};
+
+    bool insert(Vec3i idx, int row)
+    {
+        CV_Assert(row >= 0);
+
+        int bufferNum = 0;
+        int hash = int(calc_hash(idx) % hash_divisor);
+        int start = getPos(idx, bufferNum);
+        int i = start;
+
+        while (i >= 0)
+        {
+            Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
+
+            if (v->idx[0] == NAN_ELEMENT)
+            {
+                Vec4i idx4(idx[0], idx[1], idx[2], 0);
+
+                bool extend = false;
+                if (i != start && i % list_size == 0)
+                {
+                    if (bufferNum >= bufferNums - 1)
+                    {
+                        extend = true;
+                        volumes.resize(hash_divisor * bufferNums);
+                        bufferNums++;
+                    }
+                    bufferNum++;
+                    v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size;
+                }
+                else
+                {
+                    v->nextVolumeRow = i + 1;
+                }
+
+                v->idx = idx4;
+                v->row = row;
+
+                return extend;
+            }
+
+            i = v->nextVolumeRow;
+        }
+        return false;
+    }
+    int findRow(Vec3i idx) const
+    {
+        int bufferNum = 0;
+        int i = getPos(idx, bufferNum);
+
+        while (i >= 0)
+        {
+            const Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
+
+            if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0))
+                return v->row;
+            else
+                i = v->nextVolumeRow;
+        }
+
+        return -1;
+    }
+
+    inline int getPos(Vec3i idx, int bufferNum) const
+    {
+        int hash = int(calc_hash(idx) % hash_divisor);
+        return (bufferNum * hash_divisor + hash) * list_size;
+    }
+
+    inline size_t calc_hash(Vec3i x) const
+    {
+        uint32_t seed = 0;
+        constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
+        for (int i = 0; i < 3; i++)
+        {
+            seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+        }
+        return seed;
+    }
+};
+
+int calcVolumeUnitDegree(Point3i volumeResolution);
+
+typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
+
+void integrateHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth,
+    InputArray _depth, InputArray _pixNorms, InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits);
+
+void raycastHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
+    InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals);
+
+void fetchNormalsFromHashTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const int volumeUnitDegree, InputArray _points, OutputArray _normals);
+
+void fetchPointsNormalsFromHashTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
+    const int volumeUnitDegree, OutputArray _points, OutputArray _normals);
+
+#ifdef HAVE_OPENCL
+void ocl_integrateHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth,
+    InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputOutputArray _volUnitsDataCopy, InputOutputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags);
+
+void ocl_raycastHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
+    const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals);
+
+void ocl_fetchNormalsFromHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy,
+    const CustomHashSet& hashTable, InputArray _points, OutputArray _normals);
+
+void ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(
+    const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy,
+    const CustomHashSet& hashTable, OutputArray _points, OutputArray _normals);
+#endif
+
+} // namespace cv
+
+#endif

modules/ptcloud/src/io_base.cpp

@@ -0,0 +1,35 @@
+// 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.
+
+#include "precomp.hpp"
+#include "io_base.hpp"
+
+namespace cv {
+
+void BasePointCloudDecoder::setSource(const std::string& filename) noexcept
+{
+    m_filename = filename;
+}
+
+void BasePointCloudDecoder::readData(std::vector<Point3f> &points, std::vector<Point3f> &normals, std::vector<Point3f> &rgb)
+{
+    std::vector<std::vector<int32_t>> indices;
+    std::vector<Point3f> texCoords;
+    int nTexCoords;
+    readData(points, normals, rgb, texCoords, nTexCoords, indices, READ_AS_IS_FLAG);
+}
+
+void BasePointCloudEncoder::setDestination(const std::string& filename) noexcept
+{
+    m_filename = filename;
+}
+
+void BasePointCloudEncoder::writeData(const std::vector<Point3f> &points, const std::vector<Point3f> &normals, const std::vector<Point3f> &rgb)
+{
+    std::vector<std::vector<int32_t>> indices;
+    std::vector<Point3f> texCoords;
+    writeData(points, normals, rgb, texCoords, 0, indices);
+}
+
+} /* namespace cv */

modules/ptcloud/src/io_base.hpp

@@ -0,0 +1,59 @@
+// 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 _CODERS_BASE_H_
+#define _CODERS_BASE_H_
+
+#include <vector>
+#include <memory>
+#include <cstdint>
+
+#include <opencv2/core.hpp>
+
+namespace cv {
+
+class BasePointCloudDecoder;
+class BasePointCloudEncoder;
+using PointCloudDecoder = std::unique_ptr<BasePointCloudDecoder>;
+using PointCloudEncoder = std::unique_ptr<BasePointCloudEncoder>;
+
+// for OBJ files: to read vertices, normals and texture coords as they are given in the file
+// or duplicate them according to faces
+const int READ_AS_IS_FLAG = 1;
+
+///////////////////////////////// base class for decoders ////////////////////////
+class BasePointCloudDecoder
+{
+public:
+    virtual ~BasePointCloudDecoder() = default;
+
+    virtual void setSource(const std::string& filename) noexcept;
+    virtual void readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb);
+    virtual void readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb,
+                          std::vector<Point3f>& texCoords, int& nTexCoords,
+                          std::vector<std::vector<int32_t>>& indices, int flags) = 0;
+
+protected:
+    std::string m_filename;
+};
+
+///////////////////////////////// base class for encoders ////////////////////////
+class BasePointCloudEncoder
+{
+public:
+    virtual ~BasePointCloudEncoder() = default;
+
+    virtual void setDestination(const std::string& filename) noexcept;
+    virtual void writeData(const std::vector<Point3f>& points, const std::vector<Point3f>& normals, const std::vector<Point3f>& rgb);
+    virtual void writeData(const std::vector<Point3f>& points, const std::vector<Point3f>& normals, const std::vector<Point3f>& rgb,
+                           const std::vector<Point3f>& texCoords, int nTexCoords,
+                           const std::vector<std::vector<int32_t>>& indices) = 0;
+
+protected:
+    std::string m_filename;
+};
+
+} /* namespace cv */
+
+#endif

modules/ptcloud/src/io_obj.cpp

@@ -0,0 +1,293 @@
+// 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.
+
+#include "precomp.hpp"
+#include "io_obj.hpp"
+#include <fstream>
+#include <opencv2/core/utils/logger.hpp>
+#include "utils.hpp"
+
+namespace cv {
+
+std::unordered_set<std::string> ObjDecoder::m_unsupportedKeys;
+
+void ObjDecoder::readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb)
+{
+    std::vector<Point3f> texCoords;
+    int nTexCoords;
+    std::vector<std::vector<int32_t>> indices;
+    this->readData(points, normals, rgb, texCoords, nTexCoords, indices, READ_AS_IS_FLAG);
+}
+
+void ObjDecoder::readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb,
+                          std::vector<Point3f>& texCoords, int& nTexCoords,
+                          std::vector<std::vector<int32_t>>& indices, int flags)
+{
+    std::vector<Point3f> ptsList, nrmList, texCoordList, rgbList;
+    std::vector<std::vector<int32_t>> idxList, texIdxList, normalIdxList;
+
+    nTexCoords = 0;
+
+    bool duplicateVertices = false;
+
+    std::ifstream file(m_filename, std::ios::binary);
+    if (!file)
+    {
+        CV_LOG_ERROR(NULL, "Impossible to open the file: " << m_filename);
+        return;
+    }
+    std::string s;
+
+    while (!file.eof())
+    {
+        std::getline(file, s);
+        // "\r" symbols are not trimmed by default
+        s = trimSpaces(s);
+        if (s.empty())
+            continue;
+        std::stringstream ss(s);
+        std::string key;
+        ss >> key;
+
+        if (key == "#")
+            continue;
+        else if (key == "v")
+        {
+            // (x, y, z, [w], [r, g, b])
+            auto splitArr = split(s, ' ');
+            if (splitArr.size() <= 3)
+            {
+                CV_LOG_ERROR(NULL, "Vertex should have at least 3 coordinate values.");
+                return;
+            }
+            Point3f vertex;
+            ss >> vertex.x >> vertex.y >> vertex.z;
+            ptsList.push_back(vertex);
+            if (splitArr.size() == 5 || splitArr.size() == 8)
+            {
+                float w;
+                ss >> w;
+                CV_UNUSED(w);
+            }
+            if (splitArr.size() >= 7)
+            {
+                Point3f color;
+                if (ss.rdbuf()->in_avail() != 0)
+                {
+                    ss >> color.x >> color.y >> color.z;
+                    rgbList.push_back(color);
+                }
+            }
+        }
+        else if (key == "vn")
+        {
+            Point3f normal;
+            ss >> normal.x >> normal.y >> normal.z;
+            nrmList.push_back(normal);
+        }
+        else if (key == "f")
+        {
+            // format: "f v0 / t0 / n0 v1 / t1 / n1 v2/t2/n2 ..."
+            std::vector<int> vertexInd, normInd, texInd;
+            vertexInd.reserve(3); normInd.reserve(3); texInd.reserve(3);
+            auto tokens = split(s, ' ');
+            for (size_t i = 1; i < tokens.size(); i++)
+            {
+                auto vertexinfo = split(tokens[i], '/');
+                std::array<int, 3> idx = { -1, -1, -1 };
+                for (int j = 0; j < (int)vertexinfo.size(); j++)
+                {
+                    std::string sj = vertexinfo[j];
+                    // trimming spaces; as a result s can become empty - this is not an error
+                    auto si = std::find_if(sj.begin(),  sj.end(),  [](char c) { return (c >= '0' && c <= '9'); });
+                    auto ei = std::find_if(sj.rbegin(), sj.rend(), [](char c) { return (c >= '0' && c <= '9'); });
+                    if (si != sj.end() && ei != sj.rend())
+                    {
+                        auto first = std::distance(si, sj.begin());
+                        auto last  = std::distance(ei, sj.rend());
+                        sj = sj.substr(first, last - first + 1);
+                        try
+                        {
+                            idx[j] = std::stoi(sj);
+                        }
+                        // std::invalid_exception, std::out_of_range
+                        catch(const std::exception&)
+                        {
+                            CV_LOG_ERROR(NULL, "Failed to parse face index: " + sj);
+                            return;
+                        }
+                    }
+                }
+                int vertexIndex   = idx[0];
+                int texCoordIndex = idx[1];
+                int normalIndex   = idx[2];
+
+                if (vertexIndex <= 0)
+                {
+                    CV_LOG_ERROR(NULL, "Vertex index is not present or incorrect");
+                    return;
+                }
+
+                if ((vertexIndex != texCoordIndex && texCoordIndex >= 0) ||
+                    (vertexIndex != normalIndex   && normalIndex   >= 0))
+                {
+                    duplicateVertices = !(flags & READ_AS_IS_FLAG);
+                }
+
+                vertexInd.push_back(vertexIndex - 1);
+                normInd.push_back(normalIndex - 1);
+                texInd.push_back(texCoordIndex - 1);
+            }
+            idxList.push_back(vertexInd);
+            texIdxList.push_back(texInd);
+            normalIdxList.push_back(normInd);
+        }
+        else if (key == "vt")
+        {
+            // (u, [v, [w]])
+            auto splitArr = split(s, ' ');
+            int ncoords = (int)splitArr.size() - 1;
+            if (!nTexCoords)
+            {
+                nTexCoords = ncoords;
+                if (nTexCoords < 1 || nTexCoords > 3)
+                {
+                    CV_LOG_ERROR(NULL, "The amount of texture coordinates should be between 1 and 3");
+                    return;
+                }
+            }
+
+            if (ncoords != nTexCoords)
+            {
+                CV_LOG_ERROR(NULL, "All points should have the same number of texture coordinates");
+                return;
+            }
+
+            Vec3f tc;
+            for (int i = 0; i < nTexCoords; i++)
+            {
+                ss >> tc[i];
+            }
+            texCoordList.push_back(tc);
+        }
+        else
+        {
+            if (m_unsupportedKeys.find(key) == m_unsupportedKeys.end()) {
+                m_unsupportedKeys.insert(key);
+                CV_LOG_WARNING(NULL, "Key " << key << " not supported");
+            }
+        }
+    }
+
+    if (duplicateVertices)
+    {
+        points.clear();
+        normals.clear();
+        rgb.clear();
+        texCoords.clear();
+        indices.clear();
+
+        for (int tri = 0; tri < (int)idxList.size(); tri++)
+        {
+            auto vi = idxList[tri];
+            auto ti = texIdxList[tri];
+            auto ni = normalIdxList[tri];
+
+            std::vector<int32_t> newvi;
+            newvi.reserve(3);
+            for (int i = 0; i < (int)vi.size(); i++)
+            {
+                newvi.push_back((int)points.size());
+                points.push_back(ptsList.at(vi[i]));
+                if (!rgbList.empty())
+                {
+                    rgb.push_back(rgbList.at(vi[i]));
+                }
+
+                texCoords.push_back(texCoordList.at(ti[i]));
+                normals.push_back(nrmList.at(ni[i]));
+            }
+
+            indices.push_back(newvi);
+        }
+    }
+    else
+    {
+        points    = std::move(ptsList);
+        normals   = std::move(nrmList);
+        rgb       = std::move(rgbList);
+        texCoords = std::move(texCoordList);
+        indices   = std::move(idxList);
+    }
+
+    file.close();
+}
+
+void ObjEncoder::writeData(const std::vector<Point3f>& points, const std::vector<Point3f>& normals, const std::vector<Point3f>& rgb,
+                           const std::vector<Point3f>& texCoords, int nTexCoords,
+                           const std::vector<std::vector<int32_t>>& indices)
+{
+    std::ofstream file(m_filename, std::ios::binary);
+    if (!file) {
+        CV_LOG_ERROR(NULL, "Impossible to open the file: " << m_filename);
+        return;
+    }
+
+    if (!rgb.empty() && rgb.size() != points.size()) {
+        CV_LOG_ERROR(NULL, "Vertices and Colors have different size.");
+        return;
+    }
+
+    if (texCoords.empty() && nTexCoords > 0)
+    {
+        CV_LOG_ERROR(NULL, "No texture coordinates provided while having nTexCoord > 0");
+        return;
+    }
+
+    file << "# OBJ file writer" << std::endl;
+    file << "o Point_Cloud" << std::endl;
+
+    for (size_t i = 0; i < points.size(); ++i)
+    {
+        file << "v " << points[i].x << " " << points[i].y << " " << points[i].z;
+        if (!rgb.empty())
+        {
+            file << " " << rgb[i].x << " " << rgb[i].y  << " " << rgb[i].z;
+        }
+        file << std::endl;
+    }
+
+    for (const auto& normal : normals)
+    {
+        file << "vn " << normal.x << " " << normal.y << " " << normal.z << std::endl;
+    }
+
+    for (const auto& tc : texCoords)
+    {
+        file << "vt " << tc.x;
+        if (nTexCoords > 1)
+        {
+            file << " " << tc.y;
+        }
+        if (nTexCoords > 2)
+        {
+            file << " " << tc.z;
+        }
+        file << std::endl;
+    }
+
+    for (const auto& faceIndices : indices)
+    {
+        file << "f ";
+        for (const auto& index : faceIndices)
+        {
+            file << index + 1 << " ";
+        }
+        file << std::endl;
+    }
+
+    file.close();
+}
+
+} /* namespace cv */

modules/ptcloud/src/io_obj.hpp

@@ -0,0 +1,36 @@
+// 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 _CODERS_OBJ_H_
+#define _CODERS_OBJ_H_
+
+#include "io_base.hpp"
+#include <unordered_set>
+
+namespace cv {
+
+class ObjDecoder CV_FINAL : public BasePointCloudDecoder
+{
+public:
+    void readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb) CV_OVERRIDE;
+    void readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb,
+                  std::vector<Point3f>& texCoords, int& nTexCoords,
+                  std::vector<std::vector<int32_t>>& indices, int flags) CV_OVERRIDE;
+
+protected:
+    static std::unordered_set<std::string> m_unsupportedKeys;
+};
+
+class ObjEncoder CV_FINAL : public BasePointCloudEncoder
+{
+public:
+    void writeData(const std::vector<Point3f>& points, const std::vector<Point3f>& normals, const std::vector<Point3f>& rgb,
+                   const std::vector<Point3f>& texCoords, int nTexCoords,
+                   const std::vector<std::vector<int32_t>>& indices) CV_OVERRIDE;
+
+};
+
+} /* namespace cv */
+
+#endif

modules/ptcloud/src/io_ply.cpp

@@ -0,0 +1,682 @@
+// 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.
+
+#include "precomp.hpp"
+#include "io_ply.hpp"
+#include "utils.hpp"
+#include <opencv2/core/utils/logger.hpp>
+#include <fstream>
+#include <string>
+#include <iomanip>
+#include <cstddef>
+
+namespace cv {
+
+static const std::set<std::string> colorKeys = { "red", "diffuse_red", "green", "diffuse_green", "blue", "diffuse_blue" };
+
+void PlyDecoder::readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb,
+                          std::vector<Point3f>& texCoords, int& nTexCoords,
+                          std::vector<std::vector<int32_t>>& indices, int /*flags*/)
+{
+    points.clear();
+    normals.clear();
+    rgb.clear();
+    texCoords.clear();
+    indices.clear();
+    nTexCoords = 0;
+
+    std::ifstream file(m_filename, std::ios::binary);
+    if (parseHeader(file, nTexCoords))
+    {
+        parseBody(file, points, normals, rgb, texCoords, indices);
+    }
+}
+
+
+bool PlyDecoder::parseHeader(std::ifstream &file, int& nTexCoords)
+{
+    std::string s;
+    std::getline(file, s);
+    if (trimSpaces(s) != "ply")
+    {
+        CV_LOG_ERROR(NULL, "Provided file is not in PLY format");
+        return false;
+    }
+    std::getline(file, s);
+    auto splitArr = split(s, ' ');
+    // "\r" symbols are not trimmed by default
+    for (auto& e : splitArr)
+    {
+        e = trimSpaces(e);
+    }
+    if (splitArr[0] != "format")
+    {
+        CV_LOG_ERROR(NULL, "Provided file doesn't have format");
+        return false;
+    }
+    if (splitArr[1] == "ascii")
+    {
+        m_inputDataFormat = DataFormat::ASCII;
+    }
+    else if (splitArr[1] == "binary_little_endian")
+    {
+        m_inputDataFormat = DataFormat::BinaryLittleEndian;
+    }
+    else if (splitArr[1] == "binary_big_endian")
+    {
+        m_inputDataFormat = DataFormat::BinaryBigEndian;
+    }
+    else
+    {
+        CV_LOG_ERROR(NULL, "Provided PLY file format is not supported");
+        return false;
+    }
+
+    const std::map<std::string, int> dataTypes =
+    {
+        { "char",   CV_8S  }, { "int8",    CV_8S  },
+        { "uchar",  CV_8U  }, { "uint8",   CV_8U  },
+        { "short",  CV_16S }, { "int16",   CV_16S },
+        { "ushort", CV_16U }, { "uint16",  CV_16U },
+        { "int",    CV_32S }, { "int32",   CV_32S },
+        { "uint",   CV_32U }, { "uint32",  CV_32U },
+        { "float",  CV_32F }, { "float32", CV_32F },
+        { "double", CV_64F }, { "float64", CV_64F },
+    };
+
+    enum ReadElement
+    {
+        READ_OTHER  = 0,
+        READ_VERTEX = 1,
+        READ_FACE   = 2
+    };
+    ReadElement elemRead = READ_OTHER;
+    m_vertexDescription = ElementDescription();
+    m_faceDescription = ElementDescription();
+    while (std::getline(file, s))
+    {
+        if (startsWith(s, "element"))
+        {
+            std::vector<std::string> splitArrElem = split(s, ' ');
+            // "\r" symbols are not trimmed by default
+            for (auto& e : splitArrElem)
+            {
+                e = trimSpaces(e);
+            }
+            std::string elemName = splitArrElem.at(1);
+            if (elemName == "vertex")
+            {
+                elemRead = READ_VERTEX;
+                if(splitArrElem.size() != 3)
+                {
+                    CV_LOG_ERROR(NULL, "Vertex element description has " << splitArrElem.size()
+                                 << " words instead of 3");
+                    return false;
+                }
+                std::istringstream iss(splitArrElem[2]);
+                iss >> m_vertexDescription.amount;
+            }
+            else if (elemName == "face")
+            {
+                elemRead = READ_FACE;
+                if(splitArrElem.size() != 3)
+                {
+                    CV_LOG_ERROR(NULL, "Face element description has " << splitArrElem.size()
+                                 << " words instead of 3");
+                    return false;
+                }
+                std::istringstream iss(splitArrElem[2]);
+                iss >> m_faceDescription.amount;
+            }
+            else
+            {
+                elemRead = READ_OTHER;
+            }
+            continue;
+        }
+        if (startsWith(s, "property"))
+        {
+            Property property;
+            std::string elName = (elemRead == READ_VERTEX) ? "Vertex" : "Face";
+            std::vector<std::string> splitArrElem = split(s, ' ');
+            // "\r" symbols are not trimmed by default
+            for (auto& e : splitArrElem)
+            {
+                e = trimSpaces(e);
+            }
+            if (splitArrElem.size() < 3)
+            {
+                CV_LOG_ERROR(NULL, elName << " property has " << splitArrElem.size()
+                             << " words instead of at least 3");
+                return false;
+            }
+            std::string propType = splitArrElem[1];
+            if (propType == "list")
+            {
+                property.isList = true;
+                if (splitArrElem.size() < 5)
+                {
+                    CV_LOG_ERROR(NULL, elName << " property has " << splitArrElem.size()
+                                 << " words instead of at least 5");
+                    return false;
+                }
+                std::string amtTypeString = splitArrElem[2];
+                if (dataTypes.count(amtTypeString) == 0)
+                {
+                    CV_LOG_ERROR(NULL, "Property type " << amtTypeString
+                                 << " is not supported");
+                    return false;
+                }
+                else
+                {
+                    property.counterType = dataTypes.at(amtTypeString);
+                }
+                std::string idxTypeString = splitArrElem[3];
+                if (dataTypes.count(idxTypeString) == 0)
+                {
+                    CV_LOG_ERROR(NULL, "Property type " << idxTypeString
+                                 << " is not supported");
+                    return false;
+                }
+                else
+                {
+                    property.valType = dataTypes.at(idxTypeString);
+                }
+
+                property.name = splitArrElem[4];
+            }
+            else
+            {
+                property.isList = false;
+                if (dataTypes.count(propType) == 0)
+                {
+                    CV_LOG_ERROR(NULL, "Property type " << propType
+                                 << " is not supported");
+                    return false;
+                }
+                else
+                {
+                    property.valType = dataTypes.at(propType);
+                }
+                property.name = splitArrElem[2];
+            }
+
+            if (elemRead == READ_VERTEX)
+            {
+                m_vertexDescription.properties.push_back(property);
+            }
+            else if (elemRead == READ_FACE)
+            {
+                m_faceDescription.properties.push_back(property);
+            }
+
+            continue;
+        }
+        if (startsWith(s, "end_header"))
+            break;
+    }
+
+    static const std::set<std::string> texCoordKeys = { "texture_u", "s", "texture_v", "t", "texture_w" };
+
+    bool good = true;
+    m_vertexCount = m_vertexDescription.amount;
+    std::map<std::string, int> amtProps;
+    for (const auto& p : m_vertexDescription.properties)
+    {
+        bool known = false;
+        if (p.name ==  "x" || p.name ==  "y" || p.name ==  "z")
+        {
+            known = true;
+            if (p.valType != CV_32F)
+            {
+                CV_LOG_ERROR(NULL, "Vertex property " << p.name
+                                                      << " should be float");
+                good = false;
+            }
+        }
+        if (p.name == "nx" || p.name == "ny" || p.name == "nz")
+        {
+            known = true;
+            if (p.valType != CV_32F)
+            {
+                CV_LOG_ERROR(NULL, "Vertex property " << p.name
+                                                      << " should be float");
+                good = false;
+            }
+            m_hasNormal = true;
+        }
+        if (colorKeys.count(p.name) > 0)
+        {
+            known = true;
+            if (p.valType != CV_8U)
+            {
+                CV_LOG_ERROR(NULL, "Vertex property " << p.name
+                                                      << " should be uchar");
+                good = false;
+            }
+            m_hasColour = true;
+        }
+        if (texCoordKeys.count(p.name) > 0)
+        {
+            known = true;
+            if (p.valType != CV_32F)
+            {
+                CV_LOG_ERROR(NULL, "Vertex property " << p.name
+                                                      << " should be float");
+                good = false;
+            }
+            m_hasTexCoord = true;
+        }
+        if (p.isList)
+        {
+            CV_LOG_ERROR(NULL, "List properties for vertices are not supported");
+            good = false;
+        }
+        if (known)
+        {
+            amtProps[p.name]++;
+        }
+    }
+
+    // check if we have no duplicates
+    for (const auto& a : amtProps)
+    {
+        if (a.second > 1)
+        {
+            CV_LOG_ERROR(NULL, "Vertex property " << a.first << " is duplicated");
+            good = false;
+        }
+    }
+    const std::array<std::string, 3> vertKeys = {"x", "y", "z"};
+    for (const std::string& c : vertKeys)
+    {
+        if (amtProps.count(c) == 0)
+        {
+            CV_LOG_ERROR(NULL, "Vertex property " << c << " is not presented in the file");
+            good = false;
+        }
+    }
+
+    // check for synonyms
+    std::vector<std::pair<size_t, size_t>> propCounts;
+    std::vector<std::pair<std::string, std::string>> synonyms = {
+        {"red", "diffuse_red"},
+        {"green", "diffuse_green"},
+        {"blue", "diffuse_blue"},
+        {"texture_u", "s"},
+        {"texture_v", "t"},
+    };
+    for (const auto& p : synonyms)
+    {
+        std::string a, b;
+        a = p.first; b = p.second;
+        size_t ca = amtProps.count(a), cb = amtProps.count(b);
+        propCounts.push_back({ca, cb});
+        if (ca + cb > 1)
+        {
+            CV_LOG_ERROR(NULL, "Vertex property " << a << " should not go with its synonym " << b);
+            good = false;
+        }
+    }
+    // check for color conventions
+    bool shortColorConv   = propCounts[0].first  || propCounts[1].first  || propCounts[2].first;
+    bool diffuseColorConv = propCounts[0].second || propCounts[1].second || propCounts[2].second;
+    if (shortColorConv && diffuseColorConv)
+    {
+        CV_LOG_ERROR(NULL, "Vertex color properties should not be diffuse and not diffuse at the same time");
+        good = false;
+    }
+    // check for texture conventions
+    bool shortTexConv = propCounts[3].second || propCounts[4].second;
+    bool longTexConv  = propCounts[3].first  || propCounts[4].first;
+    if (shortTexConv && longTexConv)
+    {
+        CV_LOG_ERROR(NULL, "Vertex texture coordinates properties should not be in a short and in a long form at the same time");
+        good = false;
+    }
+
+    nTexCoords = 0;
+    for (const auto& k : texCoordKeys)
+    {
+        nTexCoords += (int)(amtProps.count(k));
+    }
+
+    m_faceCount = m_faceDescription.amount;
+    int amtLists = 0;
+    for (const auto& p : m_faceDescription.properties)
+    {
+        if (p.isList)
+        {
+            amtLists++;
+            if (!(p.counterType == CV_8U && (p.valType == CV_32S || p.valType == CV_32U)))
+            {
+                CV_LOG_ERROR(NULL, "List property " << p.name
+                             << " should have type uint8 for counter and uint32 for values");
+                good = false;
+            }
+            if (!(p.name == "vertex_index" || p.name == "vertex_indices"))
+            {
+                CV_LOG_ERROR(NULL, "List property should be vertex_index or vertex_indices, "
+                             << p.name << " is not supported");
+                good = false;
+            }
+        }
+    }
+    if (amtLists > 1)
+    {
+        CV_LOG_ERROR(NULL, "Only 1 list property is supported per face");
+        good = false;
+    }
+
+    return good;
+}
+
+template <typename T>
+T readNext(std::ifstream &file, DataFormat format)
+{
+    T val;
+    if (format == DataFormat::ASCII)
+    {
+        file >> val;
+        return val;
+    }
+    file.read((char *)&val, sizeof(T));
+#ifdef WORDS_BIGENDIAN
+    if (!(format == DataFormat::BinaryBigEndian) )
+    {
+        swapEndian<T>(val);
+    }
+#else
+    if (format == DataFormat::BinaryBigEndian)
+    {
+        swapEndian<T>(val);
+    }
+#endif
+    return val;
+}
+
+template <>
+uchar readNext<uchar>(std::ifstream &file, DataFormat format)
+{
+    if (format == DataFormat::ASCII)
+    {
+        int val;
+        file >> val;
+        return (uchar)val;
+    }
+    uchar val;
+    file.read((char *)&val, sizeof(uchar));
+    // 1 byte does not have to be endian-swapped
+    return val;
+}
+
+void PlyDecoder::parseBody(std::ifstream &file,
+                           std::vector<Point3f>& points, std::vector<Point3f>& normals,
+                           std::vector<Point3f>& rgb, std::vector<Point3f>& texCoords,
+                           std::vector<std::vector<int32_t>> &indices)
+{
+    points.reserve(m_vertexCount);
+    if (m_hasColour)
+    {
+        rgb.reserve(m_vertexCount);
+    }
+    if (m_hasNormal)
+    {
+        normals.reserve(m_vertexCount);
+    }
+
+    struct VertexFields
+    {
+        float vx, vy, vz;
+        float nx, ny, nz;
+        float u, v, w;
+        float r, g, b;
+    };
+
+    union VertexData
+    {
+        std::array<uchar, sizeof(VertexFields)> bytes;
+        VertexFields vf;
+    };
+
+    // to avoid string matching at file loading
+    std::vector<size_t> vertexOffsets(m_vertexDescription.properties.size(), (size_t)(-1));
+    for (size_t j = 0; j < m_vertexDescription.properties.size(); j++)
+    {
+        const auto& p = m_vertexDescription.properties[j];
+        size_t offset = (size_t)(-1);
+        if (p.name == "x")
+            offset = offsetof(VertexFields, vx);
+        if (p.name == "y")
+            offset = offsetof(VertexFields, vy);
+        if (p.name == "z")
+            offset = offsetof(VertexFields, vz);
+        if (p.name == "nx")
+            offset = offsetof(VertexFields, nx);
+        if (p.name == "ny")
+            offset = offsetof(VertexFields, ny);
+        if (p.name == "nz")
+            offset = offsetof(VertexFields, nz);
+        if (p.name == "texture_u" || p.name == "s")
+            offset = offsetof(VertexFields, u);
+        if (p.name == "texture_v" || p.name == "t")
+            offset = offsetof(VertexFields, v);
+        if (p.name == "texture_w")
+            offset = offsetof(VertexFields, w);
+        if (p.name == "red"   || p.name == "diffuse_red")
+            offset = offsetof(VertexFields, r);
+        if (p.name == "green" || p.name == "diffuse_green")
+            offset = offsetof(VertexFields, g);
+        if (p.name == "blue"  || p.name == "diffuse_blue")
+            offset = offsetof(VertexFields, b);
+        vertexOffsets[j] = offset;
+    }
+
+    for (size_t i = 0; i < m_vertexCount; i++)
+    {
+        VertexData vertexData{ };
+        for (size_t j = 0; j < m_vertexDescription.properties.size(); j++)
+        {
+            const auto& p = m_vertexDescription.properties[j];
+            uint ival = 0; float fval = 0;
+            // here signedness is not important
+            switch (p.valType)
+            {
+            case CV_8U: case CV_8S:
+                ival = readNext<uchar>(file, m_inputDataFormat);
+                break;
+            case CV_16U: case CV_16S:
+                ival = readNext<ushort>(file, m_inputDataFormat);
+                break;
+            case CV_32S: case CV_32U:
+                ival = readNext<uint>(file, m_inputDataFormat);
+                break;
+            case CV_32F:
+                fval = readNext<float>(file, m_inputDataFormat);
+                break;
+            case CV_64F:
+                fval = (float)readNext<double>(file, m_inputDataFormat);
+                break;
+            default:
+                break;
+            }
+            size_t offset = vertexOffsets[j];
+            if (offset != (size_t)(-1))
+            {
+                if (colorKeys.count(p.name) > 0 && p.valType == CV_8U)
+                {
+                    fval = ival / 255.f;
+                }
+
+                *(float*)(vertexData.bytes.data() + offset) = fval;
+            }
+        }
+
+        points.push_back({ vertexData.vf.vx, vertexData.vf.vy, vertexData.vf.vz });
+        if (m_hasColour)
+        {
+            rgb.push_back({ vertexData.vf.r, vertexData.vf.g, vertexData.vf.b });
+        }
+        if (m_hasNormal)
+        {
+            normals.push_back({ vertexData.vf.nx, vertexData.vf.ny, vertexData.vf.nz });
+        }
+        if (m_hasTexCoord)
+        {
+            texCoords.push_back({ vertexData.vf.u, vertexData.vf.v, vertexData.vf.w });
+        }
+    }
+
+    indices.reserve(m_faceCount);
+    for (size_t i = 0; i < m_faceCount; i++)
+    {
+        for (const auto& p : m_faceDescription.properties)
+        {
+            if (p.isList)
+            {
+                size_t nVerts = readNext<uchar>(file, m_inputDataFormat);
+                if (nVerts < 3)
+                {
+                    CV_LOG_ERROR(NULL, "Face should have at least 3 vertices but has " << nVerts);
+                    return;
+                }
+                // PLY can have faces with >3 vertices in TRIANGLE_FAN format
+                // in this case we load them as separate triangles
+                int vert1 = readNext<int>(file, m_inputDataFormat);
+                int vert2 = readNext<int>(file, m_inputDataFormat);
+                for (size_t j = 2; j < nVerts; j++)
+                {
+                    int vert3 = readNext<int>(file, m_inputDataFormat);
+                    indices.push_back({vert1, vert2, vert3});
+                    vert2 = vert3;
+                }
+            }
+            else
+            {
+                // read and discard
+                switch (p.valType)
+                {
+                case CV_8U: case CV_8S:
+                    readNext<uchar>(file, m_inputDataFormat);
+                    break;
+                case CV_16U: case CV_16S:
+                    readNext<ushort>(file, m_inputDataFormat);
+                    break;
+                case CV_32S: case CV_32U:
+                    readNext<uint>(file, m_inputDataFormat);
+                    break;
+                case CV_32F:
+                    readNext<float>(file, m_inputDataFormat);
+                    break;
+                case CV_64F:
+                    readNext<double>(file, m_inputDataFormat);
+                    break;
+                default:
+                    break;
+                }
+            }
+        }
+    }
+}
+
+void PlyEncoder::writeData(const std::vector<Point3f>& points, const std::vector<Point3f>& normals, const std::vector<Point3f>& rgb,
+                           const std::vector<Point3f>& texCoords, int nTexCoords,
+                           const std::vector<std::vector<int32_t>>& indices)
+{
+    std::ofstream file(m_filename, std::ios::binary);
+    if (!file)
+    {
+        CV_LOG_ERROR(NULL, "Impossible to open the file: " << m_filename);
+        return;
+    }
+    bool hasNormals = !normals.empty(), hasColor = !rgb.empty();
+    if (texCoords.empty() && nTexCoords > 0)
+    {
+        CV_LOG_ERROR(NULL, "No texture coordinates provided while having nTexCoord > 0");
+        return;
+    }
+
+    file << "ply" << std::endl;
+    file << "format ascii 1.0" << std::endl;
+    file << "comment created by OpenCV" << std::endl;
+    file << "element vertex " << points.size() << std::endl;
+
+    file << "property float x" << std::endl;
+    file << "property float y" << std::endl;
+    file << "property float z" << std::endl;
+
+    if(hasColor)
+    {
+        file << "property uchar red" << std::endl;
+        file << "property uchar green" << std::endl;
+        file << "property uchar blue" << std::endl;
+    }
+
+    if (hasNormals)
+    {
+        file << "property float nx" << std::endl;
+        file << "property float ny" << std::endl;
+        file << "property float nz" << std::endl;
+    }
+
+    if (nTexCoords > 0)
+    {
+        file << "property float texture_u" << std::endl;
+    }
+    if (nTexCoords > 1)
+    {
+        file << "property float texture_v" << std::endl;
+    }
+    if (nTexCoords > 2)
+    {
+        file << "property float texture_w" << std::endl;
+    }
+
+    if (!indices.empty())
+    {
+        file << "element face " << indices.size() << std::endl;
+        file << "property list uchar int vertex_indices" << std::endl;
+    }
+
+    file << "end_header" << std::endl;
+
+    for (size_t i = 0; i < points.size(); i++)
+    {
+        file << std::setprecision(9) << points[i].x << " " << points[i].y << " " << points[i].z;
+        if (hasColor)
+        {
+            file << " " << static_cast<int>(rgb[i].x * 255.f) << " " << static_cast<int>(rgb[i].y * 255.f) << " " << static_cast<int>(rgb[i].z * 255.f);
+        }
+        if (hasNormals)
+        {
+            file << " " << std::setprecision(9) << normals[i].x << " " << normals[i].y << " " << normals[i].z;
+        }
+        if (nTexCoords > 0)
+        {
+            file << " " << std::setprecision(9) << texCoords[i].x;
+        }
+        if (nTexCoords > 1)
+        {
+            file << " " << std::setprecision(9) << texCoords[i].y;
+        }
+        if (nTexCoords > 2)
+        {
+            file << " " << std::setprecision(9) << texCoords[i].z;
+        }
+        file << std::endl;
+    }
+
+    for (const auto& faceIndices : indices)
+    {
+        file << faceIndices.size();
+        for (const auto& index : faceIndices)
+        {
+            file << " " << index;
+        }
+        file << std::endl;
+    }
+    file.close();
+}
+
+} /* namespace cv */

modules/ptcloud/src/io_ply.hpp

@@ -0,0 +1,70 @@
+// 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 _CODERS_PLY_H_
+#define _CODERS_PLY_H_
+
+#include "io_base.hpp"
+#include <istream>
+#include <vector>
+
+namespace cv {
+
+enum class DataFormat
+{
+    ASCII,
+    BinaryLittleEndian,
+    BinaryBigEndian
+};
+
+struct Property
+{
+    bool isList;
+    int counterType;
+    int valType;
+    std::string name;
+};
+
+struct ElementDescription
+{
+    size_t amount;
+    std::vector<Property> properties;
+};
+
+class PlyDecoder CV_FINAL : public BasePointCloudDecoder
+{
+public:
+    void readData(std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb,
+                  std::vector<Point3f>& texCoords, int& nTexCoords,
+                  std::vector<std::vector<int32_t>>& indices, int flags) CV_OVERRIDE;
+
+protected:
+    bool parseHeader(std::ifstream &file, int& nTexCoords);
+    void parseBody(std::ifstream &file,
+                   std::vector<Point3f>& points, std::vector<Point3f>& normals, std::vector<Point3f>& rgb,
+                   std::vector<Point3f>& texCoords,
+                   std::vector<std::vector<int32_t>>& indices);
+
+    DataFormat m_inputDataFormat;
+    size_t m_vertexCount{0};
+    size_t m_faceCount{0};
+    bool m_hasColour{false};
+    bool m_hasNormal{false};
+    bool m_hasTexCoord{false};
+    ElementDescription m_vertexDescription;
+    ElementDescription m_faceDescription;
+};
+
+class PlyEncoder CV_FINAL : public BasePointCloudEncoder
+{
+public:
+    void writeData(const std::vector<Point3f>& points, const std::vector<Point3f>& normals, const std::vector<Point3f>& rgb,
+                   const std::vector<Point3f>& texCoords, int nTexCoords,
+                   const std::vector<std::vector<int32_t>>& indices) CV_OVERRIDE;
+
+};
+
+} /* namespace cv */
+
+#endif

modules/ptcloud/src/kinfu_frame.cpp

@@ -0,0 +1,783 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+#include "precomp.hpp"
+#include "opencv2/ptcloud/detail/kinfu_frame.hpp"
+#include "utils.hpp"
+#include "opencl_kernels_ptcloud.hpp"
+
+namespace cv {
+
+static void computePointsNormals(const cv::Intr, float depthFactor, const Depth, Points, Normals );
+void computePointsNormalsColors(const Intr, const Intr, float, const Depth, const Colors, Points, Normals, Colors);
+static Depth pyrDownBilateral(const Depth depth, float sigma);
+static void pyrDownPointsNormals(const Points p, const Normals n, Points& pdown, Normals& ndown);
+
+template<int p>
+inline float specPow(float x)
+{
+    if(p % 2 == 0)
+    {
+        float v = specPow<p/2>(x);
+        return v*v;
+    }
+    else
+    {
+        float v = specPow<(p-1)/2>(x);
+        return v*v*x;
+    }
+}
+
+template<>
+inline float specPow<0>(float /*x*/)
+{
+    return 1.f;
+}
+
+template<>
+inline float specPow<1>(float x)
+{
+    return x;
+}
+
+
+struct RenderInvoker : ParallelLoopBody
+{
+    RenderInvoker(const Points& _points, const Normals& _normals, Mat_<Vec4b>& _img, Vec3f _lightPt, Size _sz) :
+        ParallelLoopBody(),
+        points(_points),
+        normals(_normals),
+        img(_img),
+        lightPt(_lightPt),
+        sz(_sz)
+    { }
+
+    virtual void operator ()(const Range& range) const override
+    {
+        for(int y = range.start; y < range.end; y++)
+        {
+            Vec4b* imgRow = img[y];
+            const ptype* ptsRow = points[y];
+            const ptype* nrmRow = normals[y];
+
+            for(int x = 0; x < sz.width; x++)
+            {
+                Point3f p = fromPtype(ptsRow[x]);
+                Point3f n = fromPtype(nrmRow[x]);
+
+                Vec4b color;
+
+                if(isNaN(p))
+                {
+                    color = Vec4b(0, 32, 0, 0);
+                }
+                else
+                {
+                    const float Ka = 0.3f;  //ambient coeff
+                    const float Kd = 0.5f;  //diffuse coeff
+                    const float Ks = 0.2f;  //specular coeff
+                    const int   sp = 20;  //specular power
+
+                    const float Ax = 1.f;   //ambient color,  can be RGB
+                    const float Dx = 1.f;   //diffuse color,  can be RGB
+                    const float Sx = 1.f;   //specular color, can be RGB
+                    const float Lx = 1.f;   //light color
+
+                    Point3f l = normalize(lightPt - Vec3f(p));
+                    Point3f v = normalize(-Vec3f(p));
+                    Point3f r = normalize(Vec3f(2.f*n*n.dot(l) - l));
+
+                    uchar ix = (uchar)((Ax*Ka*Dx + Lx*Kd*Dx*max(0.f, n.dot(l)) +
+                                        Lx*Ks*Sx*specPow<sp>(max(0.f, r.dot(v))))*255.f);
+                    color = Vec4b(ix, ix, ix, 0);
+                }
+
+                imgRow[x] = color;
+            }
+        }
+    }
+
+    const Points& points;
+    const Normals& normals;
+    Mat_<Vec4b>& img;
+    Vec3f lightPt;
+    Size sz;
+};
+
+struct RenderColorInvoker : ParallelLoopBody
+{
+    RenderColorInvoker(const Points& _points, const Colors& _colors, Mat_<Vec4b>& _img, Size _sz) :
+        ParallelLoopBody(),
+        points(_points),
+        colors(_colors),
+        img(_img),
+        sz(_sz)
+    { }
+
+    virtual void operator ()(const Range& range) const override
+    {
+        for(int y = range.start; y < range.end; y++)
+        {
+            Vec4b* imgRow = img[y];
+            const ptype* ptsRow = points[y];
+            const ptype* clrRow = colors[y];
+
+            for(int x = 0; x < sz.width; x++)
+            {
+                Point3f p = fromPtype(ptsRow[x]);
+                Point3f c = fromPtype(clrRow[x]);
+                Vec4b color;
+
+                if(isNaN(p) || isNaN(c))
+                {
+                    color = Vec4b(0, 32, 0, 0);
+                }
+                else
+                {
+                    color = Vec4b((uchar)c.x, (uchar)c.y, (uchar)c.z, (uchar)0);
+                }
+
+                imgRow[x] = color;
+            }
+        }
+    }
+
+    const Points& points;
+    const Colors& colors;
+    Mat_<Vec4b>& img;
+    Size sz;
+};
+
+
+void pyrDownPointsNormals(const Points p, const Normals n, Points &pdown, Normals &ndown)
+{
+    CV_TRACE_FUNCTION();
+
+    for(int y = 0; y < pdown.rows; y++)
+    {
+        ptype* ptsRow = pdown[y];
+        ptype* nrmRow = ndown[y];
+        const ptype* pUpRow0 = p[2*y];
+        const ptype* pUpRow1 = p[2*y+1];
+        const ptype* nUpRow0 = n[2*y];
+        const ptype* nUpRow1 = n[2*y+1];
+        for(int x = 0; x < pdown.cols; x++)
+        {
+            Point3f point = nan3, normal = nan3;
+
+            Point3f d00 = fromPtype(pUpRow0[2*x]);
+            Point3f d01 = fromPtype(pUpRow0[2*x+1]);
+            Point3f d10 = fromPtype(pUpRow1[2*x]);
+            Point3f d11 = fromPtype(pUpRow1[2*x+1]);
+
+            if(!(isNaN(d00) || isNaN(d01) || isNaN(d10) || isNaN(d11)))
+            {
+                point = (d00 + d01 + d10 + d11)*0.25f;
+
+                Point3f n00 = fromPtype(nUpRow0[2*x]);
+                Point3f n01 = fromPtype(nUpRow0[2*x+1]);
+                Point3f n10 = fromPtype(nUpRow1[2*x]);
+                Point3f n11 = fromPtype(nUpRow1[2*x+1]);
+
+                normal = (n00 + n01 + n10 + n11)*0.25f;
+            }
+
+            ptsRow[x] = toPtype(point);
+            nrmRow[x] = toPtype(normal);
+        }
+    }
+}
+
+struct PyrDownBilateralInvoker : ParallelLoopBody
+{
+    PyrDownBilateralInvoker(const Depth& _depth, Depth& _depthDown, float _sigma) :
+        ParallelLoopBody(),
+        depth(_depth),
+        depthDown(_depthDown),
+        sigma(_sigma)
+    { }
+
+    virtual void operator ()(const Range& range) const override
+    {
+        float sigma3 = sigma*3;
+        const int D = 5;
+
+        for(int y = range.start; y < range.end; y++)
+        {
+            depthType* downRow = depthDown[y];
+            const depthType* srcCenterRow = depth[2*y];
+
+            for(int x = 0; x < depthDown.cols; x++)
+            {
+                depthType center = srcCenterRow[2*x];
+
+                int sx = max(0, 2*x - D/2), ex = min(2*x - D/2 + D, depth.cols-1);
+                int sy = max(0, 2*y - D/2), ey = min(2*y - D/2 + D, depth.rows-1);
+
+                depthType sum = 0;
+                int count = 0;
+
+                for(int iy = sy; iy < ey; iy++)
+                {
+                    const depthType* srcRow = depth[iy];
+                    for(int ix = sx; ix < ex; ix++)
+                    {
+                        depthType val = srcRow[ix];
+                        if(abs(val - center) < sigma3)
+                        {
+                            sum += val; count ++;
+                        }
+                    }
+                }
+
+                downRow[x] = (count == 0) ? 0 : sum / count;
+            }
+        }
+    }
+
+    const Depth& depth;
+    Depth& depthDown;
+    float sigma;
+};
+
+
+Depth pyrDownBilateral(const Depth depth, float sigma)
+{
+    CV_TRACE_FUNCTION();
+
+    Depth depthDown(depth.rows/2, depth.cols/2);
+
+    PyrDownBilateralInvoker pdi(depth, depthDown, sigma);
+    Range range(0, depthDown.rows);
+    const int nstripes = -1;
+    parallel_for_(range, pdi, nstripes);
+
+    return depthDown;
+}
+
+struct ComputePointsNormalsInvoker : ParallelLoopBody
+{
+    ComputePointsNormalsInvoker(const Depth& _depth, Points& _points, Normals& _normals,
+                                const Intr::Reprojector& _reproj, float _dfac) :
+        ParallelLoopBody(),
+        depth(_depth),
+        points(_points),
+        normals(_normals),
+        reproj(_reproj),
+        dfac(_dfac)
+    { }
+
+    virtual void operator ()(const Range& range) const override
+    {
+        for(int y = range.start; y < range.end; y++)
+        {
+            const depthType* depthRow0 = depth[y];
+            const depthType* depthRow1 = (y < depth.rows - 1) ? depth[y + 1] : 0;
+            ptype    *ptsRow = points[y];
+            ptype   *normRow = normals[y];
+
+            for(int x = 0; x < depth.cols; x++)
+            {
+                depthType d00 = depthRow0[x];
+                depthType z00 = d00*dfac;
+                Point3f v00 = reproj(Point3f((float)x, (float)y, z00));
+
+                Point3f p = nan3, n = nan3;
+
+                if(x < depth.cols - 1 && y < depth.rows - 1)
+                {
+                    depthType d01 = depthRow0[x+1];
+                    depthType d10 = depthRow1[x];
+
+                    depthType z01 = d01*dfac;
+                    depthType z10 = d10*dfac;
+
+                    // before it was
+                    //if(z00*z01*z10 != 0)
+                    if(z00 != 0 && z01 != 0 && z10 != 0)
+                    {
+                        Point3f v01 = reproj(Point3f((float)(x+1), (float)(y+0), z01));
+                        Point3f v10 = reproj(Point3f((float)(x+0), (float)(y+1), z10));
+
+                        cv::Vec3f vec = (v01-v00).cross(v10-v00);
+                        n = -normalize(vec);
+                        p = v00;
+                    }
+                }
+
+                ptsRow[x] = toPtype(p);
+                normRow[x] = toPtype(n);
+            }
+        }
+    }
+
+    const Depth& depth;
+    Points& points;
+    Normals& normals;
+    const Intr::Reprojector& reproj;
+    float dfac;
+};
+
+
+void computePointsNormals(const Intr intr, float depthFactor, const Depth depth,
+                          Points points, Normals normals)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(!points.empty() && !normals.empty());
+    CV_Assert(depth.size() == points.size());
+    CV_Assert(depth.size() == normals.size());
+
+    // conversion to meters
+    // before it was:
+    //float dfac = 0.001f/depthFactor;
+    float dfac = 1.f/depthFactor;
+
+    Intr::Reprojector reproj = intr.makeReprojector();
+
+    ComputePointsNormalsInvoker ci(depth, points, normals, reproj, dfac);
+    Range range(0, depth.rows);
+    const int nstripes = -1;
+    parallel_for_(range, ci, nstripes);
+}
+
+
+///////// GPU implementation /////////
+
+#ifdef HAVE_OPENCL
+
+static bool ocl_renderPointsNormals(const UMat points, const UMat normals, UMat image, Vec3f lightLoc);
+static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, OutputArrayOfArrays normals,
+                                   const Intr intr, int levels, float depthFactor,
+                                   float sigmaDepth, float sigmaSpatial, int kernelSize,
+                                   float truncateThreshold);
+static bool ocl_buildPyramidPointsNormals(const UMat points, const UMat normals,
+                                          OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
+                                          int levels);
+
+static bool computePointsNormalsGpu(const Intr intr, float depthFactor, const UMat& depth, UMat& points, UMat& normals);
+static bool pyrDownBilateralGpu(const UMat& depth, UMat& depthDown, float sigma);
+static bool customBilateralFilterGpu(const UMat src, UMat& dst, int kernelSize, float sigmaDepth, float sigmaSpatial);
+static bool pyrDownPointsNormalsGpu(const UMat p, const UMat n, UMat &pdown, UMat &ndown);
+
+
+bool computePointsNormalsGpu(const Intr intr, float depthFactor, const UMat& depth,
+                             UMat& points, UMat& normals)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(!points.empty() && !normals.empty());
+    CV_Assert(depth.size() == points.size());
+    CV_Assert(depth.size() == normals.size());
+    CV_Assert(depth.type() == DEPTH_TYPE);
+    CV_Assert(points.type()  == POINT_TYPE);
+    CV_Assert(normals.type() == POINT_TYPE);
+
+    // conversion to meters
+    float dfac = 1.f/depthFactor;
+
+    Intr::Reprojector reproj = intr.makeReprojector();
+
+    cv::String errorStr;
+    cv::String name = "computePointsNormals";
+    ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
+    cv::String options = "-cl-mad-enable";
+    ocl::Kernel k;
+    k.create(name.c_str(), source, options, &errorStr);
+
+    if(k.empty())
+        return false;
+
+    Vec2f fxyinv(reproj.fxinv, reproj.fyinv), cxy(reproj.cx, reproj.cy);
+
+    k.args(ocl::KernelArg::WriteOnlyNoSize(points),
+           ocl::KernelArg::WriteOnlyNoSize(normals),
+           ocl::KernelArg::ReadOnly(depth),
+           fxyinv.val,
+           cxy.val,
+           dfac);
+
+    size_t globalSize[2];
+    globalSize[0] = (size_t)depth.cols;
+    globalSize[1] = (size_t)depth.rows;
+
+    return k.run(2, globalSize, NULL, true);
+}
+
+
+bool pyrDownBilateralGpu(const UMat& depth, UMat& depthDown, float sigma)
+{
+    CV_TRACE_FUNCTION();
+
+    depthDown.create(depth.rows/2, depth.cols/2, DEPTH_TYPE);
+
+    cv::String errorStr;
+    cv::String name = "pyrDownBilateral";
+    ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
+    cv::String options = "-cl-mad-enable";
+    ocl::Kernel k;
+    k.create(name.c_str(), source, options, &errorStr);
+
+    if(k.empty())
+        return false;
+
+    k.args(ocl::KernelArg::ReadOnly(depth),
+           ocl::KernelArg::WriteOnly(depthDown),
+           sigma);
+
+    size_t globalSize[2];
+    globalSize[0] = (size_t)depthDown.cols;
+    globalSize[1] = (size_t)depthDown.rows;
+
+    return k.run(2, globalSize, NULL, true);
+}
+
+//TODO: remove it when OpenCV's bilateral processes 32f on GPU
+bool customBilateralFilterGpu(const UMat src /* udepth */, UMat& dst /* smooth */,
+                              int kernelSize, float sigmaDepth, float sigmaSpatial)
+{
+    CV_TRACE_FUNCTION();
+
+    Size frameSize = src.size();
+
+    CV_Assert(frameSize.area() > 0);
+    CV_Assert(src.type() == DEPTH_TYPE);
+
+    dst.create(frameSize, DEPTH_TYPE);
+
+    cv::String errorStr;
+    cv::String name = "customBilateral";
+    ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
+    cv::String options = "-cl-mad-enable";
+    ocl::Kernel k;
+    k.create(name.c_str(), source, options, &errorStr);
+
+    if(k.empty())
+        return false;
+
+    k.args(ocl::KernelArg::ReadOnlyNoSize(src),
+           ocl::KernelArg::WriteOnlyNoSize(dst),
+           frameSize,
+           kernelSize,
+           0.5f / (sigmaSpatial * sigmaSpatial),
+           0.5f / (sigmaDepth * sigmaDepth));
+
+    size_t globalSize[2];
+    globalSize[0] = (size_t)src.cols;
+    globalSize[1] = (size_t)src.rows;
+
+    return k.run(2, globalSize, NULL, true);
+}
+
+
+bool pyrDownPointsNormalsGpu(const UMat p, const UMat n, UMat &pdown, UMat &ndown)
+{
+    CV_TRACE_FUNCTION();
+
+    cv::String errorStr;
+    cv::String name = "pyrDownPointsNormals";
+    ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
+    cv::String options = "-cl-mad-enable";
+    ocl::Kernel k;
+    k.create(name.c_str(), source, options, &errorStr);
+
+    if(k.empty())
+        return false;
+
+    Size downSize = pdown.size();
+
+    k.args(ocl::KernelArg::ReadOnlyNoSize(p),
+           ocl::KernelArg::ReadOnlyNoSize(n),
+           ocl::KernelArg::WriteOnlyNoSize(pdown),
+           ocl::KernelArg::WriteOnlyNoSize(ndown),
+           downSize);
+
+    size_t globalSize[2];
+    globalSize[0] = (size_t)pdown.cols;
+    globalSize[1] = (size_t)pdown.rows;
+
+    return k.run(2, globalSize, NULL, true);
+}
+
+
+static bool ocl_renderPointsNormals(const UMat points, const UMat normals,
+                                    UMat img, Vec3f lightLoc)
+{
+    CV_TRACE_FUNCTION();
+
+    cv::String errorStr;
+    cv::String name = "render";
+    ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
+    cv::String options = "-cl-mad-enable";
+    ocl::Kernel k;
+    k.create(name.c_str(), source, options, &errorStr);
+
+    if(k.empty())
+        return false;
+
+    Vec4f lightPt(lightLoc[0], lightLoc[1], lightLoc[2]);
+    Size frameSize = points.size();
+
+    k.args(ocl::KernelArg::ReadOnlyNoSize(points),
+           ocl::KernelArg::ReadOnlyNoSize(normals),
+           ocl::KernelArg::WriteOnlyNoSize(img),
+           frameSize,
+           lightPt.val);
+
+    size_t globalSize[2];
+    globalSize[0] = (size_t)points.cols;
+    globalSize[1] = (size_t)points.rows;
+
+    return k.run(2, globalSize, NULL, true);
+}
+
+
+static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, OutputArrayOfArrays normals,
+                                   const Intr intr, int levels, float depthFactor,
+                                   float sigmaDepth, float sigmaSpatial, int kernelSize,
+                                   float truncateThreshold)
+{
+    CV_TRACE_FUNCTION();
+
+    // looks like OpenCV's bilateral filter works the same as KinFu's
+    UMat smooth;
+    //TODO: fix that
+    // until 32f isn't implemented in OpenCV in OpenCL, we should use our workarounds
+    //bilateralFilter(udepth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial);
+    if(!customBilateralFilterGpu(depth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial))
+        return false;
+
+    // depth truncation can be used in some scenes
+    UMat depthThreshold;
+    if(truncateThreshold > 0.f)
+        threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV);
+    else
+        depthThreshold = smooth;
+
+    UMat scaled = depthThreshold;
+    Size sz = smooth.size();
+    points.create(levels, 1, POINT_TYPE);
+    normals.create(levels, 1, POINT_TYPE);
+    for(int i = 0; i < levels; i++)
+    {
+        UMat& p = points.getUMatRef(i);
+        UMat& n = normals.getUMatRef(i);
+        p.create(sz, POINT_TYPE);
+        n.create(sz, POINT_TYPE);
+
+        if(!computePointsNormalsGpu(intr.scale(i), depthFactor, scaled, p, n))
+            return false;
+
+        if(i < levels - 1)
+        {
+            sz.width /= 2, sz.height /= 2;
+            UMat halfDepth(sz, DEPTH_TYPE);
+            pyrDownBilateralGpu(scaled, halfDepth, sigmaDepth*depthFactor);
+            scaled = halfDepth;
+        }
+    }
+
+    return true;
+}
+
+
+static bool ocl_buildPyramidPointsNormals(const UMat points, const UMat normals,
+                                          OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
+                                          int levels)
+{
+    CV_TRACE_FUNCTION();
+
+    pyrPoints .create(levels, 1, POINT_TYPE);
+    pyrNormals.create(levels, 1, POINT_TYPE);
+
+    pyrPoints .getUMatRef(0) = points;
+    pyrNormals.getUMatRef(0) = normals;
+
+    Size sz = points.size();
+    for(int i = 1; i < levels; i++)
+    {
+        UMat p1 = pyrPoints .getUMat(i-1);
+        UMat n1 = pyrNormals.getUMat(i-1);
+
+        sz.width /= 2; sz.height /= 2;
+        UMat& p0 = pyrPoints .getUMatRef(i);
+        UMat& n0 = pyrNormals.getUMatRef(i);
+        p0.create(sz, POINT_TYPE);
+        n0.create(sz, POINT_TYPE);
+
+        if(!pyrDownPointsNormalsGpu(p1, n1, p0, n0))
+            return false;
+    }
+
+    return true;
+}
+
+#endif
+
+namespace detail {
+
+void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Vec3f lightLoc)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(_points.size().area() > 0);
+    CV_Assert(_points.size() == _normals.size());
+
+    Size sz = _points.size();
+    image.create(sz, CV_8UC4);
+
+    CV_OCL_RUN(_points.isUMat() && _normals.isUMat() && image.isUMat(),
+               ocl_renderPointsNormals(_points.getUMat(),
+                                       _normals.getUMat(),
+                                       image.getUMat(), lightLoc))
+
+    Points  points  = _points.getMat();
+    Normals normals = _normals.getMat();
+
+    Mat_<Vec4b> img = image.getMat();
+
+    RenderInvoker ri(points, normals, img, lightLoc, sz);
+    Range range(0, sz.height);
+    const int nstripes = -1;
+    parallel_for_(range, ri, nstripes);
+}
+
+void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(_points.size().area() > 0);
+    CV_Assert(_points.size() == _normals.size());
+
+    Size sz = _points.size();
+    image.create(sz, CV_8UC4);
+
+    Points  points  = _points.getMat();
+    Normals normals = _normals.getMat();
+    Colors colors = _colors.getMat();
+
+    Mat_<Vec4b> img = image.getMat();
+
+    RenderColorInvoker ri(points, colors, img, sz);
+    Range range(0, sz.height);
+    const int nstripes = -1;
+    parallel_for_(range, ri, nstripes);
+}
+
+} // namespace detail
+
+void makeFrameFromDepth(InputArray _depth,
+                        OutputArray pyrPoints, OutputArray pyrNormals,
+                        const Matx33f _intr, int levels, float depthFactor,
+                        float sigmaDepth, float sigmaSpatial, int kernelSize,
+                        float truncateThreshold)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(_depth.type() == DEPTH_TYPE);
+
+    Intr intr(_intr);
+    CV_OCL_RUN(_depth.isUMat() && pyrPoints.isUMatVector() && pyrNormals.isUMatVector(),
+               ocl_makeFrameFromDepth(_depth.getUMat(), pyrPoints, pyrNormals,
+                                      intr, levels, depthFactor,
+                                      sigmaDepth, sigmaSpatial, kernelSize,
+                                      truncateThreshold));
+
+    int kp = pyrPoints.kind(), kn = pyrNormals.kind();
+    // There can be UMats in the container (when OpenCL is off)
+    CV_Assert(kp == _InputArray::STD_ARRAY_MAT || kp == _InputArray::STD_VECTOR_MAT || kp == _InputArray::STD_VECTOR_UMAT);
+    CV_Assert(kn == _InputArray::STD_ARRAY_MAT || kn == _InputArray::STD_VECTOR_MAT || kp == _InputArray::STD_VECTOR_UMAT);
+
+    Depth depth = _depth.getMat();
+
+    // looks like OpenCV's bilateral filter works the same as KinFu's
+    Depth smooth;
+    Depth depthNoNans = depth.clone();
+    patchNaNs(depthNoNans);
+    bilateralFilter(depthNoNans, smooth, kernelSize, sigmaDepth * depthFactor, sigmaSpatial);
+
+    // depth truncation can be used in some scenes
+    Depth depthThreshold;
+    if(truncateThreshold > 0.f)
+        threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV);
+    else
+        depthThreshold = smooth;
+
+    // we don't need depth pyramid outside this method
+    // if we do, the code is to be refactored
+
+    Depth scaled = depthThreshold;
+    Size sz = smooth.size();
+    pyrPoints.create(levels, 1, POINT_TYPE);
+    pyrNormals.create(levels, 1, POINT_TYPE);
+    for(int i = 0; i < levels; i++)
+    {
+        pyrPoints .create(sz, POINT_TYPE, i);
+        pyrNormals.create(sz, POINT_TYPE, i);
+
+        // There can be UMats in the container (when OpenCL is off),
+        // getMatRef() is not applicable
+        Points  p = pyrPoints. getMat(i);
+        Normals n = pyrNormals.getMat(i);
+
+        computePointsNormals(intr.scale(i), depthFactor, scaled, p, n);
+
+        if(i < levels - 1)
+        {
+            sz.width /= 2; sz.height /= 2;
+            scaled = pyrDownBilateral(scaled, sigmaDepth*depthFactor);
+        }
+    }
+}
+
+
+void buildPyramidPointsNormals(InputArray _points, InputArray _normals,
+                               OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
+                               int levels)
+{
+    CV_TRACE_FUNCTION();
+
+    CV_Assert(_points.type() == POINT_TYPE);
+    CV_Assert(_points.type() == _normals.type());
+    CV_Assert(_points.size() == _normals.size());
+
+    CV_OCL_RUN(_points.isUMat() && _normals.isUMat() &&
+               pyrPoints.isUMatVector() && pyrNormals.isUMatVector(),
+               ocl_buildPyramidPointsNormals(_points.getUMat(), _normals.getUMat(),
+                                             pyrPoints, pyrNormals,
+                                             levels));
+
+    int kp = pyrPoints.kind(), kn = pyrNormals.kind();
+    CV_Assert(kp == _InputArray::STD_ARRAY_MAT || kp == _InputArray::STD_VECTOR_MAT);
+    CV_Assert(kn == _InputArray::STD_ARRAY_MAT || kn == _InputArray::STD_VECTOR_MAT);
+
+    Mat p0 = _points.getMat(), n0 = _normals.getMat();
+
+    pyrPoints .create(levels, 1, POINT_TYPE);
+    pyrNormals.create(levels, 1, POINT_TYPE);
+
+    pyrPoints .getMatRef(0) = p0;
+    pyrNormals.getMatRef(0) = n0;
+
+    Size sz = _points.size();
+    for(int i = 1; i < levels; i++)
+    {
+        Points  p1 = pyrPoints .getMat(i-1);
+        Normals n1 = pyrNormals.getMat(i-1);
+
+        sz.width /= 2; sz.height /= 2;
+
+        pyrPoints .create(sz, POINT_TYPE, i);
+        pyrNormals.create(sz, POINT_TYPE, i);
+        Points  pd = pyrPoints. getMatRef(i);
+        Normals nd = pyrNormals.getMatRef(i);
+
+        pyrDownPointsNormals(p1, n1, pd, nd);
+    }
+}
+
+} // namespace cv

modules/ptcloud/src/load_point_cloud.cpp

@@ -0,0 +1,341 @@
+// 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.
+
+#include "precomp.hpp"
+
+#include "io_base.hpp"
+#include "io_obj.hpp"
+#include "io_ply.hpp"
+#include "utils.hpp"
+#include "opencv2/core/utils/filesystem.private.hpp"
+#include <opencv2/core/utils/logger.hpp>
+
+#include <memory>
+
+namespace cv {
+
+#if OPENCV_HAVE_FILESYSTEM_SUPPORT
+
+static PointCloudDecoder findDecoder(const String &filename)
+{
+    auto file_ext = getExtension(filename);
+    if (file_ext == "obj" || file_ext == "OBJ")
+    {
+        return std::unique_ptr<ObjDecoder>(new ObjDecoder());
+    }
+    if (file_ext == "ply" || file_ext == "PLY")
+    {
+        return std::unique_ptr<PlyDecoder>(new PlyDecoder());
+    }
+
+    return nullptr;
+}
+
+static PointCloudEncoder findEncoder(const String &filename)
+{
+    auto file_ext = getExtension(filename);
+    if (file_ext == "obj" || file_ext == "OBJ")
+    {
+        return std::unique_ptr<ObjEncoder>(new ObjEncoder());
+    }
+    if (file_ext == "ply" || file_ext == "PLY")
+    {
+        return std::unique_ptr<PlyEncoder>(new PlyEncoder());
+    }
+
+    return nullptr;
+}
+
+#endif
+
+void loadPointCloud(const String &filename, OutputArray vertices, OutputArray normals, OutputArray rgb)
+{
+#if OPENCV_HAVE_FILESYSTEM_SUPPORT
+    auto decoder = findDecoder(filename);
+    if (!decoder) {
+        String file_ext = getExtension(filename);
+        CV_LOG_ERROR(NULL, "File extension '" << file_ext << "' is not supported");
+        return;
+    }
+
+    decoder->setSource(filename);
+
+    std::vector<Point3f> vec_vertices, vec_normals, vec_rgb;
+
+    decoder->readData(vec_vertices, vec_normals, vec_rgb);
+
+    if (!vec_vertices.empty())
+        Mat(static_cast<int>(vec_vertices.size()), 1, CV_32FC3, vec_vertices.data()).copyTo(vertices);
+
+    if (!vec_normals.empty() && normals.needed())
+        Mat(static_cast<int>(vec_normals.size()), 1, CV_32FC3, vec_normals.data()).copyTo(normals);
+
+    if (!vec_rgb.empty() && rgb.needed())
+        Mat(static_cast<int>(vec_rgb.size()), 1, CV_32FC3, vec_rgb.data()).copyTo(rgb);
+
+#else // OPENCV_HAVE_FILESYSTEM_SUPPORT
+    CV_UNUSED(filename);
+    CV_UNUSED(vertices);
+    CV_UNUSED(normals);
+    CV_UNUSED(rgb);
+    CV_LOG_WARNING(NULL, "File system support is disabled in this OpenCV build!");
+#endif
+}
+
+void savePointCloud(const String &filename, InputArray vertices, InputArray normals, InputArray rgb)
+{
+#if OPENCV_HAVE_FILESYSTEM_SUPPORT
+    if (vertices.empty()) {
+        CV_LOG_WARNING(NULL, "Have no vertices to save");
+        return;
+    };
+
+    auto encoder = findEncoder(filename);
+    if (!encoder) {
+        String file_ext = getExtension(filename);
+        CV_LOG_ERROR(NULL, "File extension '" << file_ext << "' is not supported");
+        return;
+    }
+
+    encoder->setDestination(filename);
+
+    std::vector<Point3f> vec_vertices(vertices.getMat()), vec_normals, vec_rgb;
+
+    if (!normals.empty())
+    {
+        vec_normals = normals.getMat();
+    }
+
+    if (!rgb.empty())
+    {
+        vec_rgb = rgb.getMat();
+    }
+    encoder->writeData(vec_vertices, vec_normals, vec_rgb);
+
+#else // OPENCV_HAVE_FILESYSTEM_SUPPORT
+    CV_UNUSED(filename);
+    CV_UNUSED(vertices);
+    CV_UNUSED(normals);
+    CV_UNUSED(rgb);
+    CV_LOG_WARNING(NULL, "File system support is disabled in this OpenCV build!");
+#endif
+}
+
+void loadMesh(const String &filename, OutputArray vertices, OutputArrayOfArrays indices,
+              OutputArray normals, OutputArray colors, OutputArray texCoords)
+{
+#if OPENCV_HAVE_FILESYSTEM_SUPPORT
+    CV_Assert(vertices.needed());
+    CV_Assert(indices.needed());
+
+    PointCloudDecoder decoder = findDecoder(filename);
+    String file_ext = getExtension(filename);
+    if (!decoder) {
+        CV_LOG_ERROR(NULL, "File extension '" << file_ext << "' is not supported");
+        return;
+    }
+
+    decoder->setSource(filename);
+
+    std::vector<Point3f> vec_vertices, vec_normals, vec_rgb;
+    std::vector<std::vector<int32_t>> vec_indices;
+
+    std::vector<Point3f> vec_texCoords;
+    int nTexCoords = 0;
+
+    decoder->readData(vec_vertices, vec_normals, vec_rgb, vec_texCoords, nTexCoords, vec_indices, 0);
+
+    if (!vec_vertices.empty())
+    {
+        Mat(1, static_cast<int>(vec_vertices.size()), CV_32FC3, vec_vertices.data()).copyTo(vertices);
+    }
+
+    if (normals.needed() && !vec_normals.empty())
+    {
+        Mat(1, static_cast<int>(vec_normals.size()), CV_32FC3, vec_normals.data()).copyTo(normals);
+    }
+
+    if (colors.needed() && !vec_rgb.empty())
+    {
+        Mat(1, static_cast<int>(vec_rgb.size()), CV_32FC3, vec_rgb.data()).copyTo(colors);
+    }
+
+    if (!vec_indices.empty())
+    {
+        _InputArray::KindFlag kind = indices.kind();
+        int vecsz = (int)vec_indices.size();
+        if (kind == _InputArray::KindFlag::STD_VECTOR_VECTOR)
+        {
+            CV_Assert(indices.depth() == CV_32S);
+            std::vector<std::vector<int32_t>>& vec = *(std::vector<std::vector<int32_t>>*)indices.getObj();
+            vec.resize(vecsz);
+            for (int i = 0; i < vecsz; ++i)
+            {
+                Mat(1, static_cast<int>(vec_indices[i].size()), CV_32SC1, vec_indices[i].data()).copyTo(vec[i]);
+            }
+        }
+        // std::array<Mat> has fixed size, unsupported
+        else if (kind == _InputArray::KindFlag::STD_VECTOR_MAT)
+        {
+            indices.create(vecsz, 1, CV_32S);
+            for (int i = 0; i < vecsz; i++)
+            {
+                std::vector<int> vi = vec_indices[i];
+                indices.create(1, (int)vi.size(), CV_32S, i);
+                Mat(vi).copyTo(indices.getMat(i));
+            }
+        }
+        else
+        {
+            std::vector<Vec3i> vec(vec_indices.size());
+            for (int i = 0; i < vecsz; ++i)
+            {
+                Vec3i tri;
+                size_t sz = vec_indices[i].size();
+                if (sz != 3)
+                {
+                    CV_Error(Error::StsBadArg, "Face contains " + std::to_string(sz) + " vertices, can not put it into 3-channel indices array");
+                }
+                else
+                {
+                    for (int j = 0; j < 3; j++)
+                    {
+                        tri[j] = vec_indices[i][j];
+                    }
+                }
+                vec[i] = tri;
+            }
+            indices.create(1, (int)vec_indices.size(), CV_32SC3);
+            Mat(1, static_cast<int>(vec_indices.size()), CV_32SC3, vec.data()).copyTo(indices);
+        }
+    }
+
+    if (texCoords.needed())
+    {
+        if (nTexCoords)
+        {
+            CV_Assert(!texCoords.fixedType() || (texCoords.type() == CV_MAKE_TYPE(CV_32F, nTexCoords)));
+
+            Mat tex3(vec_texCoords);
+
+            if (nTexCoords == 3)
+            {
+                tex3.copyTo(texCoords);
+            }
+            else if (nTexCoords == 2)
+            {
+                // if texCoords is empty then channels() can be any number
+                bool has3ch = texCoords.channels() == 3;
+                int ch = has3ch ? 3 : 2;
+                std::vector<int> permut = has3ch ? std::vector<int>{ 0, 0, 1, 1, -1, 2 } : std::vector<int>{ 0, 0, 1, 1 };
+                texCoords.createSameSize(vec_texCoords, CV_MAKE_TYPE(CV_32F, ch));
+                Mat out = texCoords.getMat();
+                cv::mixChannels(tex3, out, permut);
+            }
+        }
+        else
+        {
+            texCoords.clear();
+        }
+    }
+
+#else // OPENCV_HAVE_FILESYSTEM_SUPPORT
+    CV_UNUSED(filename);
+    CV_UNUSED(vertices);
+    CV_UNUSED(normals);
+    CV_UNUSED(colors);
+    CV_UNUSED(indices);
+    CV_UNUSED(texCoords);
+    CV_LOG_WARNING(NULL, "File system support is disabled in this OpenCV build!");
+#endif
+}
+
+void saveMesh(const String &filename, InputArray vertices, InputArrayOfArrays indices,
+              InputArray normals, InputArray colors, InputArray texCoords)
+{
+#if OPENCV_HAVE_FILESYSTEM_SUPPORT
+    if (vertices.empty()) {
+        CV_LOG_WARNING(NULL, "Have no vertices to save");
+        return;
+    }
+
+    auto encoder = findEncoder(filename);
+    String file_ext = getExtension(filename);
+    if (!encoder) {
+        CV_LOG_ERROR(NULL, "File extension '" << file_ext << "' is not supported");
+        return;
+    }
+
+    encoder->setDestination(filename);
+
+    std::vector<Point3f> vec_vertices(vertices.getMat()), vec_normals, vec_rgb;
+    if (!normals.empty())
+    {
+        vec_normals = normals.getMat();
+    }
+
+    if (!colors.empty())
+    {
+        vec_rgb = colors.getMat();
+    }
+
+    std::vector<std::vector<int32_t>> vec_indices;
+    CV_Assert(indices.depth() == CV_32S);
+    if (indices.kind() == _InputArray::KindFlag::STD_VECTOR_VECTOR ||
+        indices.kind() == _InputArray::KindFlag::STD_VECTOR_MAT)
+    {
+        std::vector<Mat> mat_indices;
+        indices.getMatVector(mat_indices);
+        vec_indices.resize(mat_indices.size());
+        for (size_t i = 0; i < mat_indices.size(); ++i)
+        {
+            mat_indices[i].copyTo(vec_indices[i]);
+        }
+    }
+    else
+    {
+        CV_Assert(indices.channels() == 3);
+        std::vector<Vec3i>& vec = *(std::vector<Vec3i>*)indices.getObj();
+        vec_indices.resize(vec.size());
+        for (size_t i = 0; i < vec.size(); ++i)
+        {
+            for (int j = 0; j < 3; j++)
+            {
+                vec_indices[i].push_back(vec[i][j]);
+            }
+        }
+    }
+
+    std::vector<Point3f> vec_texCoords;
+    int nTexCoords = 0;
+    if (!texCoords.empty())
+    {
+        nTexCoords = texCoords.channels();
+    }
+    if (nTexCoords == 2)
+    {
+        // extend by 3rd zero channel
+        vec_texCoords.resize(texCoords.total());
+        cv::mixChannels(texCoords, vec_texCoords, {0, 0, 1, 1, -1, 2});
+    }
+    if (nTexCoords == 3)
+    {
+        texCoords.copyTo(vec_texCoords);
+    }
+
+    encoder->writeData(vec_vertices, vec_normals, vec_rgb, vec_texCoords, nTexCoords, vec_indices);
+
+#else // OPENCV_HAVE_FILESYSTEM_SUPPORT
+    CV_UNUSED(filename);
+    CV_UNUSED(vertices);
+    CV_UNUSED(colors);
+    CV_UNUSED(normals);
+    CV_UNUSED(indices);
+    CV_UNUSED(texCoords);
+    CV_LOG_WARNING(NULL, "File system support is disabled in this OpenCV build!");
+#endif
+
+}
+
+}/* namespace cv */

modules/ptcloud/src/normal.cpp

@@ -0,0 +1,921 @@
+// 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
+
+#include "precomp.hpp"
+
+namespace cv
+{
+
+/** Just compute the norm of a vector
+ * @param vec a vector of size 3 and any type T
+ * @return
+ */
+template<typename T>
+T inline norm_vec(const Vec<T, 3>& vec)
+{
+    return std::sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
+}
+template<typename T>
+T inline norm_vec(const Vec<T, 4>& vec)
+{
+    return std::sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
+}
+
+/** Given 3d points, compute their distance to the origin
+ * @param points
+ * @return
+ */
+template<typename T>
+Mat_<T> computeRadius(const Mat& points)
+{
+    typedef Vec<T, 4> PointT;
+
+    // Compute the
+    Size size(points.cols, points.rows);
+    Mat_<T> r(size);
+    if (points.isContinuous())
+        size = Size(points.cols * points.rows, 1);
+    for (int y = 0; y < size.height; ++y)
+    {
+        const PointT* point = points.ptr < PointT >(y), * point_end = points.ptr < PointT >(y) + size.width;
+        T* row = r[y];
+        for (; point != point_end; ++point, ++row)
+            *row = norm_vec(*point);
+    }
+
+    return r;
+}
+
+// Compute theta and phi according to equation 3 of
+// ``Fast and Accurate Computation of Surface Normals from Range Images``
+// by H. Badino, D. Huber, Y. Park and T. Kanade
+template<typename T>
+void computeThetaPhi(int rows, int cols, const Matx<T, 3, 3>& K, Mat& cos_theta, Mat& sin_theta,
+                     Mat& cos_phi, Mat& sin_phi)
+{
+    // Create some bogus coordinates
+    Mat depth_image = K(0, 0) * Mat_<T> ::ones(rows, cols);
+    Mat points3d;
+    depthTo3d(depth_image, Mat(K), points3d);
+
+    //typedef Vec<T, 3> Vec3T;
+    typedef Vec<T, 4> Vec4T;
+
+    cos_theta = Mat_<T>(rows, cols);
+    sin_theta = Mat_<T>(rows, cols);
+    cos_phi = Mat_<T>(rows, cols);
+    sin_phi = Mat_<T>(rows, cols);
+    Mat r = computeRadius<T>(points3d);
+    for (int y = 0; y < rows; ++y)
+    {
+        T* row_cos_theta = cos_theta.ptr <T>(y), * row_sin_theta = sin_theta.ptr <T>(y);
+        T* row_cos_phi = cos_phi.ptr <T>(y), * row_sin_phi = sin_phi.ptr <T>(y);
+        const Vec4T* row_points = points3d.ptr <Vec4T>(y),
+               * row_points_end = points3d.ptr <Vec4T>(y) + points3d.cols;
+        const T* row_r = r.ptr < T >(y);
+        for (; row_points < row_points_end;
+            ++row_cos_theta, ++row_sin_theta, ++row_cos_phi, ++row_sin_phi, ++row_points, ++row_r)
+        {
+            // In the paper, z goes away from the camera, y goes down, x goes right
+            // OpenCV has the same conventions
+            // Theta goes from z to x (and actually goes from -pi/2 to pi/2, phi goes from z to y
+            float theta = (float)std::atan2(row_points->val[0], row_points->val[2]);
+            *row_cos_theta = std::cos(theta);
+            *row_sin_theta = std::sin(theta);
+            float phi = (float)std::asin(row_points->val[1] / (*row_r));
+            *row_cos_phi = std::cos(phi);
+            *row_sin_phi = std::sin(phi);
+        }
+    }
+}
+
+/** Modify normals to make sure they point towards the camera
+ * @param normals
+ */
+template<typename T>
+inline void signNormal(const Vec<T, 3>& normal_in, Vec<T, 3>& normal_out)
+{
+    Vec<T, 3> res;
+    if (normal_in[2] > 0)
+        res = -normal_in / norm_vec(normal_in);
+    else
+        res = normal_in / norm_vec(normal_in);
+
+    normal_out[0] = res[0];
+    normal_out[1] = res[1];
+    normal_out[2] = res[2];
+}
+template<typename T>
+inline void signNormal(const Vec<T, 3>& normal_in, Vec<T, 4>& normal_out)
+{
+    Vec<T, 3> res;
+    if (normal_in[2] > 0)
+        res = -normal_in / norm_vec(normal_in);
+    else
+        res = normal_in / norm_vec(normal_in);
+
+    normal_out[0] = res[0];
+    normal_out[1] = res[1];
+    normal_out[2] = res[2];
+    normal_out[3] = 0;
+}
+
+/** Modify normals to make sure they point towards the camera
+ * @param normals
+ */
+template<typename T>
+inline void signNormal(T a, T b, T c, Vec<T, 3>& normal)
+{
+    T norm = 1 / std::sqrt(a * a + b * b + c * c);
+    if (c > 0)
+    {
+        normal[0] = -a * norm;
+        normal[1] = -b * norm;
+        normal[2] = -c * norm;
+    }
+    else
+    {
+        normal[0] = a * norm;
+        normal[1] = b * norm;
+        normal[2] = c * norm;
+    }
+}
+template<typename T>
+inline void signNormal(T a, T b, T c, Vec<T, 4>& normal)
+{
+    T norm = 1 / std::sqrt(a * a + b * b + c * c);
+    if (c > 0)
+    {
+        normal[0] = -a * norm;
+        normal[1] = -b * norm;
+        normal[2] = -c * norm;
+        normal[3] = 0;
+    }
+    else
+    {
+        normal[0] = a * norm;
+        normal[1] = b * norm;
+        normal[2] = c * norm;
+        normal[3] = 0;
+    }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<typename T>
+class RgbdNormalsImpl : public RgbdNormals
+{
+public:
+    static const int dtype = cv::traits::Depth<T>::value;
+
+    RgbdNormalsImpl(int _rows, int _cols, int _windowSize, const Mat& _K, RgbdNormals::RgbdNormalsMethod _method) :
+        rows(_rows),
+        cols(_cols),
+        windowSize(_windowSize),
+        method(_method),
+        cacheIsDirty(true)
+    {
+        CV_Assert(_K.cols == 3 && _K.rows == 3);
+
+        _K.convertTo(K, dtype);
+        _K.copyTo(K_ori);
+    }
+
+    virtual ~RgbdNormalsImpl() CV_OVERRIDE
+    { }
+
+    virtual int getDepth() const CV_OVERRIDE
+    {
+        return dtype;
+    }
+    virtual int getRows() const CV_OVERRIDE
+    {
+        return rows;
+    }
+    virtual void setRows(int val) CV_OVERRIDE
+    {
+        rows = val; cacheIsDirty = true;
+    }
+    virtual int getCols() const CV_OVERRIDE
+    {
+        return cols;
+    }
+    virtual void setCols(int val) CV_OVERRIDE
+    {
+        cols = val; cacheIsDirty = true;
+    }
+    virtual int getWindowSize() const CV_OVERRIDE
+    {
+        return windowSize;
+    }
+    virtual void setWindowSize(int val) CV_OVERRIDE
+    {
+        windowSize = val; cacheIsDirty = true;
+    }
+    virtual void getK(OutputArray val) const CV_OVERRIDE
+    {
+        K.copyTo(val);
+    }
+    virtual void setK(InputArray val) CV_OVERRIDE
+    {
+        K = val.getMat(); cacheIsDirty = true;
+    }
+    virtual RgbdNormalsMethod getMethod() const CV_OVERRIDE
+    {
+        return method;
+    }
+
+    virtual void compute(const Mat& in, Mat& normals) const = 0;
+
+    /** Given a set of 3d points in a depth image, compute the normals at each point
+     * @param points3d_in depth a float depth image. Or it can be rows x cols x 3 is they are 3d points
+     * @param normals a rows x cols x 3 matrix
+     */
+    virtual void apply(InputArray points3d_in, OutputArray normals_out) const CV_OVERRIDE
+    {
+        Mat points3d_ori = points3d_in.getMat();
+
+        CV_Assert(points3d_ori.dims == 2);
+
+        // Either we have 3d points or a depth image
+
+        bool ptsAre4F = (points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
+        bool ptsAreDepth = (points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
+        if (method == RGBD_NORMALS_METHOD_FALS || method == RGBD_NORMALS_METHOD_SRI || method == RGBD_NORMALS_METHOD_CROSS_PRODUCT)
+        {
+            if (!ptsAre4F)
+            {
+                CV_Error(Error::StsBadArg, "Input image should have 4 float-point channels");
+            }
+        }
+        else if (method == RGBD_NORMALS_METHOD_LINEMOD)
+        {
+            if (!ptsAre4F && !ptsAreDepth)
+            {
+                CV_Error(Error::StsBadArg, "Input image should have 4 float-point channels or have 1 ushort or float-point channel");
+            }
+        }
+        else
+        {
+            CV_Error(Error::StsInternal, "Unknown normal computer algorithm");
+        }
+
+        // Initialize the pimpl
+        cache();
+
+        // Precompute something for RGBD_NORMALS_METHOD_SRI and RGBD_NORMALS_METHOD_FALS
+        Mat points3d;
+        if (method != RGBD_NORMALS_METHOD_LINEMOD)
+        {
+            // Make the points have the right depth
+            if (points3d_ori.depth() == dtype)
+                points3d = points3d_ori;
+            else
+                points3d_ori.convertTo(points3d, dtype);
+        }
+
+        // Get the normals
+        normals_out.create(points3d_ori.size(), CV_MAKETYPE(dtype, 4));
+        if (points3d_ori.empty())
+            return;
+
+        Mat normals = normals_out.getMat();
+        if ((method == RGBD_NORMALS_METHOD_FALS) || (method == RGBD_NORMALS_METHOD_SRI))
+        {
+            // Compute the distance to the points
+            Mat radius = computeRadius<T>(points3d);
+            compute(radius, normals);
+        }
+        else if (method == RGBD_NORMALS_METHOD_LINEMOD)
+        {
+            compute(points3d_ori, normals);
+        }
+        else if (method == RGBD_NORMALS_METHOD_CROSS_PRODUCT)
+        {
+            compute(points3d, normals);
+        }
+        else
+        {
+            CV_Error(Error::StsInternal, "Unknown normal computer algorithm");
+        }
+    }
+
+    int rows, cols;
+    Mat K, K_ori;
+    int windowSize;
+    RgbdNormalsMethod method;
+    mutable bool cacheIsDirty;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Given a set of 3d points in a depth image, compute the normals at each point
+ * using the FALS method described in
+ * ``Fast and Accurate Computation of Surface Normals from Range Images``
+ * by H. Badino, D. Huber, Y. Park and T. Kanade
+ */
+template<typename T>
+class FALS : public RgbdNormalsImpl<T>
+{
+public:
+    typedef Matx<T, 3, 3> Mat33T;
+    typedef Vec<T, 9> Vec9T;
+    typedef Vec<T, 4> Vec4T;
+    typedef Vec<T, 3> Vec3T;
+
+    FALS(int _rows, int _cols, int _windowSize, const Mat& _K) :
+        RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_FALS)
+    { }
+    virtual ~FALS() CV_OVERRIDE
+    { }
+
+    /** Compute cached data
+     */
+    virtual void cache() const CV_OVERRIDE
+    {
+        if (!this->cacheIsDirty)
+            return;
+
+        // Compute theta and phi according to equation 3
+        Mat cos_theta, sin_theta, cos_phi, sin_phi;
+        computeThetaPhi<T>(this->rows, this->cols, this->K, cos_theta, sin_theta, cos_phi, sin_phi);
+
+        // Compute all the v_i for every points
+        std::vector<Mat> channels(3);
+        channels[0] = sin_theta.mul(cos_phi);
+        channels[1] = sin_phi;
+        channels[2] = cos_theta.mul(cos_phi);
+        merge(channels, V_);
+
+        // Compute M
+        Mat_<Vec9T> M(this->rows, this->cols);
+        Mat33T VVt;
+        const Vec3T* vec = V_[0];
+        Vec9T* M_ptr = M[0], * M_ptr_end = M_ptr + this->rows * this->cols;
+        for (; M_ptr != M_ptr_end; ++vec, ++M_ptr)
+        {
+            VVt = (*vec) * vec->t();
+            *M_ptr = Vec9T(VVt.val);
+        }
+
+        boxFilter(M, M, M.depth(), Size(this->windowSize, this->windowSize), Point(-1, -1), false);
+
+        // Compute M's inverse
+        Mat33T M_inv;
+        M_inv_.create(this->rows, this->cols);
+        Vec9T* M_inv_ptr = M_inv_[0];
+        for (M_ptr = &M(0); M_ptr != M_ptr_end; ++M_inv_ptr, ++M_ptr)
+        {
+            // We have a semi-definite matrix
+            invert(Mat33T(M_ptr->val), M_inv, DECOMP_CHOLESKY);
+            *M_inv_ptr = Vec9T(M_inv.val);
+        }
+
+        this->cacheIsDirty = false;
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    virtual void compute(const Mat& r, Mat& normals) const CV_OVERRIDE
+    {
+        // Compute B
+        Mat_<Vec3T> B(this->rows, this->cols);
+
+        const T* row_r = r.ptr < T >(0), * row_r_end = row_r + this->rows * this->cols;
+        const Vec3T* row_V = V_[0];
+        Vec3T* row_B = B[0];
+        for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V)
+        {
+            Vec3T val = (*row_V) / (*row_r);
+            if (cvIsInf(val[0]) || cvIsNaN(val[0]) ||
+                cvIsInf(val[1]) || cvIsNaN(val[1]) ||
+                cvIsInf(val[2]) || cvIsNaN(val[2]))
+                *row_B = Vec3T();
+            else
+                *row_B = val;
+        }
+
+        // Apply a box filter to B
+        boxFilter(B, B, B.depth(), Size(this->windowSize, this->windowSize), Point(-1, -1), false);
+
+        // compute the Minv*B products
+        row_r = r.ptr < T >(0);
+        const Vec3T* B_vec = B[0];
+        const Mat33T* M_inv = reinterpret_cast<const Mat33T*>(M_inv_.ptr(0));
+        //Vec3T* normal = normals.ptr<Vec3T>(0);
+        Vec4T* normal = normals.ptr<Vec4T>(0);
+        for (; row_r != row_r_end; ++row_r, ++B_vec, ++normal, ++M_inv)
+            if (cvIsNaN(*row_r))
+            {
+                (*normal)[0] = *row_r;
+                (*normal)[1] = *row_r;
+                (*normal)[2] = *row_r;
+                (*normal)[3] = 0;
+            }
+            else
+            {
+                Mat33T Mr = *M_inv;
+                Vec3T Br = *B_vec;
+                Vec3T MBr(Mr(0, 0) * Br[0] + Mr(0, 1) * Br[1] + Mr(0, 2) * Br[2],
+                          Mr(1, 0) * Br[0] + Mr(1, 1) * Br[1] + Mr(1, 2) * Br[2],
+                          Mr(2, 0) * Br[0] + Mr(2, 1) * Br[1] + Mr(2, 2) * Br[2]);
+                signNormal(MBr, *normal);
+            }
+    }
+
+    // Cached data
+    mutable Mat_<Vec3T> V_;
+    mutable Mat_<Vec9T> M_inv_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Function that multiplies K_inv by a vector. It is just meant to speed up the product as we know
+ * that K_inv is upper triangular and K_inv(2,2)=1
+ * @param K_inv
+ * @param a
+ * @param b
+ * @param c
+ * @param res
+ */
+template<typename T, typename U>
+void multiply_by_K_inv(const Matx<T, 3, 3>& K_inv, U a, U b, U c, Vec<T, 3>& res)
+{
+    res[0] = (T)(K_inv(0, 0) * a + K_inv(0, 1) * b + K_inv(0, 2) * c);
+    res[1] = (T)(K_inv(1, 1) * b + K_inv(1, 2) * c);
+    res[2] = (T)c;
+}
+
+/** Given a depth image, compute the normals as detailed in the LINEMOD paper
+ * ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
+ * by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
+ */
+template<typename T>
+class LINEMOD : public RgbdNormalsImpl<T>
+{
+public:
+    typedef Vec<T, 4> Vec4T;
+    typedef Vec<T, 3> Vec3T;
+    typedef Matx<T, 3, 3> Mat33T;
+
+    LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K, double _diffThr = 50.0) :
+        RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD),
+        differenceThreshold(_diffThr)
+    { }
+
+    /** Compute cached data
+     */
+    virtual void cache() const CV_OVERRIDE
+    {
+        this->cacheIsDirty = false;
+    }
+
+    /** Compute the normals
+     * @param r
+     * @param normals the output normals
+     */
+    virtual void compute(const Mat& points3d, Mat& normals) const CV_OVERRIDE
+    {
+        // Only focus on the depth image for LINEMOD
+        Mat depth_in;
+        //if (points3d.channels() == 3)
+        if (points3d.channels() == 4)
+        {
+            std::vector<Mat> channels;
+            split(points3d, channels);
+            depth_in = channels[2];
+        }
+        else
+            depth_in = points3d;
+
+        switch (depth_in.depth())
+        {
+        case CV_16U:
+        {
+            const Mat_<unsigned short>& d(depth_in);
+            computeImpl<unsigned short, long>(d, normals);
+            break;
+        }
+        case CV_32F:
+        {
+            const Mat_<float>& d(depth_in);
+            computeImpl<float, float>(d, normals);
+            break;
+        }
+        case CV_64F:
+        {
+            const Mat_<double>& d(depth_in);
+            computeImpl<double, double>(d, normals);
+            break;
+        }
+        }
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    template<typename DepthDepth, typename ContainerDepth>
+    Mat computeImpl(const Mat_<DepthDepth>& depthIn, Mat& normals) const
+    {
+        const int r = 5; // used to be 7
+        const int sample_step = r;
+        const int square_size = ((2 * r / sample_step) + 1);
+        long offsets[square_size * square_size];
+        long offsets_x[square_size * square_size];
+        long offsets_y[square_size * square_size];
+        long offsets_x_x[square_size * square_size];
+        long offsets_x_y[square_size * square_size];
+        long offsets_y_y[square_size * square_size];
+        for (int j = -r, index = 0; j <= r; j += sample_step)
+            for (int i = -r; i <= r; i += sample_step, ++index)
+            {
+                offsets_x[index] = i;
+                offsets_y[index] = j;
+                offsets_x_x[index] = i * i;
+                offsets_x_y[index] = i * j;
+                offsets_y_y[index] = j * j;
+                offsets[index] = j * this->cols + i;
+            }
+
+        // Define K_inv by hand, just for higher accuracy
+        Mat33T K_inv = Matx<T, 3, 3>::eye(), kmat;
+        this->K.copyTo(kmat);
+        K_inv(0, 0) = 1.0f / kmat(0, 0);
+        K_inv(0, 1) = -kmat(0, 1) / (kmat(0, 0) * kmat(1, 1));
+        K_inv(0, 2) = (kmat(0, 1) * kmat(1, 2) - kmat(0, 2) * kmat(1, 1)) / (kmat(0, 0) * kmat(1, 1));
+        K_inv(1, 1) = 1 / kmat(1, 1);
+        K_inv(1, 2) = -kmat(1, 2) / kmat(1, 1);
+
+        Vec3T X1_minus_X, X2_minus_X;
+
+        ContainerDepth difference_threshold((ContainerDepth)differenceThreshold);
+        //TODO: fixit, difference threshold should not depend on input type
+        difference_threshold *= (ContainerDepth)(std::is_same<DepthDepth, ushort>::value ? 1000.0 : 1.0);
+        normals.setTo(std::numeric_limits<DepthDepth>::quiet_NaN());
+        for (int y = r; y < this->rows - r - 1; ++y)
+        {
+            const DepthDepth* p_line = reinterpret_cast<const DepthDepth*>(depthIn.ptr(y, r));
+            Vec4T* normal = normals.ptr<Vec4T>(y, r);
+
+            for (int x = r; x < this->cols - r - 1; ++x)
+            {
+                DepthDepth d = p_line[0];
+
+                // accum
+                long A[4];
+                A[0] = A[1] = A[2] = A[3] = 0;
+                ContainerDepth b[2];
+                b[0] = b[1] = 0;
+                for (unsigned int i = 0; i < square_size * square_size; ++i) {
+                    // We need to cast to ContainerDepth in case we have unsigned DepthDepth
+                    ContainerDepth delta = ContainerDepth(p_line[offsets[i]]) - ContainerDepth(d);
+                    if (std::abs(delta) > difference_threshold)
+                        continue;
+
+                    A[0] += offsets_x_x[i];
+                    A[1] += offsets_x_y[i];
+                    A[3] += offsets_y_y[i];
+                    b[0] += offsets_x[i] * delta;
+                    b[1] += offsets_y[i] * delta;
+                }
+
+                // solve for the optimal gradient D of equation (8)
+                long det = A[0] * A[3] - A[1] * A[1];
+                // We should divide the following two by det, but instead, we multiply
+                // X1_minus_X and X2_minus_X by det (which does not matter as we normalize the normals)
+                // Therefore, no division is done: this is only for speedup
+                ContainerDepth dx = (A[3] * b[0] - A[1] * b[1]);
+                ContainerDepth dy = (-A[1] * b[0] + A[0] * b[1]);
+
+                // Compute the dot product
+                //Vec3T X = K_inv * Vec3T(x, y, 1) * depth(y, x);
+                //Vec3T X1 = K_inv * Vec3T(x + 1, y, 1) * (depth(y, x) + dx);
+                //Vec3T X2 = K_inv * Vec3T(x, y + 1, 1) * (depth(y, x) + dy);
+                //Vec3T nor = (X1 - X).cross(X2 - X);
+                multiply_by_K_inv(K_inv, d * det + (x + 1) * dx, y * dx, dx, X1_minus_X);
+                multiply_by_K_inv(K_inv, x * dy, d * det + (y + 1) * dy, dy, X2_minus_X);
+                Vec3T nor = X1_minus_X.cross(X2_minus_X);
+                signNormal(nor, *normal);
+
+                ++p_line;
+                ++normal;
+            }
+        }
+
+        return normals;
+    }
+
+    double differenceThreshold;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Given a set of 3d points in a depth image, compute the normals at each point
+ * using the SRI method described in
+ * ``Fast and Accurate Computation of Surface Normals from Range Images``
+ * by H. Badino, D. Huber, Y. Park and T. Kanade
+ */
+template<typename T>
+class SRI : public RgbdNormalsImpl<T>
+{
+public:
+    typedef Matx<T, 3, 3> Mat33T;
+    typedef Vec<T, 9> Vec9T;
+    typedef Vec<T, 4> Vec4T;
+    typedef Vec<T, 3> Vec3T;
+
+    SRI(int _rows, int _cols, int _windowSize, const Mat& _K) :
+        RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_SRI),
+        phi_step_(0),
+        theta_step_(0)
+    { }
+
+    /** Compute cached data
+     */
+    virtual void cache() const CV_OVERRIDE
+    {
+        if (!this->cacheIsDirty)
+            return;
+
+        Mat_<T> cos_theta, sin_theta, cos_phi, sin_phi;
+        computeThetaPhi<T>(this->rows, this->cols, this->K, cos_theta, sin_theta, cos_phi, sin_phi);
+
+        // Create the derivative kernels
+        getDerivKernels(kx_dx_, ky_dx_, 1, 0, this->windowSize, true, this->dtype);
+        getDerivKernels(kx_dy_, ky_dy_, 0, 1, this->windowSize, true, this->dtype);
+
+        // Get the mapping function for SRI
+        float min_theta = (float)std::asin(sin_theta(0, 0)), max_theta = (float)std::asin(sin_theta(0, this->cols - 1));
+        float min_phi = (float)std::asin(sin_phi(0, this->cols / 2 - 1)), max_phi = (float)std::asin(sin_phi(this->rows - 1, this->cols / 2 - 1));
+
+        std::vector<Point3f> points3d(this->cols * this->rows);
+        R_hat_.create(this->rows, this->cols);
+        phi_step_ = float(max_phi - min_phi) / (this->rows - 1);
+        theta_step_ = float(max_theta - min_theta) / (this->cols - 1);
+        for (int phi_int = 0, k = 0; phi_int < this->rows; ++phi_int)
+        {
+            float phi = min_phi + phi_int * phi_step_;
+            float phi_sin = std::sin(phi), phi_cos = std::cos(phi);
+            for (int theta_int = 0; theta_int < this->cols; ++theta_int, ++k)
+            {
+                float theta = min_theta + theta_int * theta_step_;
+                float theta_sin = std::sin(theta), theta_cos = std::cos(theta);
+                // Store the 3d point to project it later
+                Point3f pp(theta_sin * phi_cos, phi_sin, theta_cos * phi_cos);
+                points3d[k] = pp;
+
+                // Cache the rotation matrix and negate it
+                Matx<T, 3, 3> mat = Matx<T, 3, 3> (0, 1, 0,  0, 0, 1,  1, 0, 0) *
+                                    Matx<T, 3, 3> (theta_cos, -theta_sin, 0,  theta_sin, theta_cos, 0,  0, 0, 1) *
+                                    Matx<T, 3, 3> (phi_cos, 0, -phi_sin,  0, 1, 0,  phi_sin, 0, phi_cos);
+
+                for (unsigned char i = 0; i < 3; ++i)
+                    mat(i, 1) = mat(i, 1) / phi_cos;
+                // The second part of the matrix is never explained in the paper ... but look at the wikipedia normal article
+                mat(0, 0) = mat(0, 0) - 2 * pp.x;
+                mat(1, 0) = mat(1, 0) - 2 * pp.y;
+                mat(2, 0) = mat(2, 0) - 2 * pp.z;
+
+                R_hat_(phi_int, theta_int) = Vec9T(mat.val);
+            }
+        }
+
+        map_.create(this->rows, this->cols);
+        projectPoints(points3d, Mat(3, 1, CV_32FC1, Scalar::all(0.0f)), Mat(3, 1, CV_32FC1, Scalar::all(0.0f)), this->K, Mat(), map_);
+        map_ = map_.reshape(2, this->rows);
+        convertMaps(map_, Mat(), xy_, fxy_, CV_16SC2);
+
+        //map for converting from Spherical coordinate space to Euclidean space
+        euclideanMap_.create(this->rows, this->cols);
+        Matx<T, 3, 3> km(this->K);
+        float invFx = (float)(1.0f / km(0, 0)), cx = (float)(km(0, 2));
+        double invFy = 1.0f / (km(1, 1)), cy = km(1, 2);
+        for (int i = 0; i < this->rows; i++)
+        {
+            float y = (float)((i - cy) * invFy);
+            for (int j = 0; j < this->cols; j++)
+            {
+                float x = (j - cx) * invFx;
+                float theta = std::atan(x);
+                float phi = std::asin(y / std::sqrt(x * x + y * y + 1.0f));
+
+                euclideanMap_(i, j) = Vec2f((theta - min_theta) / theta_step_, (phi - min_phi) / phi_step_);
+            }
+        }
+        //convert map to 2 maps in short format for increasing speed in remap function
+        convertMaps(euclideanMap_, Mat(), invxy_, invfxy_, CV_16SC2);
+
+        // Update the kernels: the steps are due to the fact that derivatives will be computed on a grid where
+        // the step is not 1. Only need to do it on one dimension as it computes derivatives in only one direction
+        kx_dx_ /= theta_step_;
+        ky_dy_ /= phi_step_;
+
+        this->cacheIsDirty = false;
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    virtual void compute(const Mat& in, Mat& normals_out) const CV_OVERRIDE
+    {
+        const Mat_<T>& r_non_interp = in;
+
+        // Interpolate the radial image to make derivatives meaningful
+        Mat_<T> r;
+        // higher quality remapping does not help here
+        remap(r_non_interp, r, xy_, fxy_, INTER_LINEAR);
+
+        // Compute the derivatives with respect to theta and phi
+        // TODO add bilateral filtering (as done in kinfu)
+        Mat_<T> r_theta, r_phi;
+        sepFilter2D(r, r_theta, r.depth(), kx_dx_, ky_dx_);
+        //current OpenCV version sometimes corrupts r matrix after second call of sepFilter2D
+        //it depends on resolution, be careful
+        sepFilter2D(r, r_phi, r.depth(), kx_dy_, ky_dy_);
+
+        // Fill the result matrix
+        Mat_<Vec4T> normals(this->rows, this->cols);
+
+        const T* r_theta_ptr = r_theta[0], * r_theta_ptr_end = r_theta_ptr + this->rows * this->cols;
+        const T* r_phi_ptr = r_phi[0];
+        const Mat33T* R = reinterpret_cast<const Mat33T*>(R_hat_[0]);
+        const T* r_ptr = r[0];
+        Vec4T* normal = normals[0];
+        for (; r_theta_ptr != r_theta_ptr_end; ++r_theta_ptr, ++r_phi_ptr, ++R, ++r_ptr, ++normal)
+        {
+            if (cvIsNaN(*r_ptr))
+            {
+                (*normal)[0] = *r_ptr;
+                (*normal)[1] = *r_ptr;
+                (*normal)[2] = *r_ptr;
+                (*normal)[3] = 0;
+            }
+            else
+            {
+                T r_theta_over_r = (*r_theta_ptr) / (*r_ptr);
+                T r_phi_over_r = (*r_phi_ptr) / (*r_ptr);
+                // R(1,1) is 0
+                signNormal((*R)(0, 0) + (*R)(0, 1) * r_theta_over_r + (*R)(0, 2) * r_phi_over_r,
+                           (*R)(1, 0) + (*R)(1, 2) * r_phi_over_r,
+                           (*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal);
+            }
+        }
+
+        remap(normals, normals_out, invxy_, invfxy_, INTER_LINEAR);
+        normal = normals_out.ptr<Vec4T>(0);
+        Vec4T* normal_end = normal + this->rows * this->cols;
+        for (; normal != normal_end; ++normal)
+            signNormal((*normal)[0], (*normal)[1], (*normal)[2], *normal);
+    }
+
+    // Cached data
+    /** Stores R */
+    mutable Mat_<Vec9T> R_hat_;
+    mutable float phi_step_, theta_step_;
+
+    /** Derivative kernels */
+    mutable Mat kx_dx_, ky_dx_, kx_dy_, ky_dy_;
+    /** mapping function to get an SRI image */
+    mutable Mat_<Vec2f> map_;
+    mutable Mat xy_, fxy_;
+
+    mutable Mat_<Vec2f> euclideanMap_;
+    mutable Mat invxy_, invfxy_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/* Uses the simpliest possible method for normals calculation: calculates cross product between two vectors
+(pointAt(x+1, y) - pointAt(x, y)) and (pointAt(x, y+1) - pointAt(x, y)) */
+
+template<typename DataType>
+class CrossProduct : public RgbdNormalsImpl<DataType>
+{
+public:
+    typedef Vec<DataType, 3> Vec3T;
+    typedef Vec<DataType, 4> Vec4T;
+    typedef Point3_<DataType> Point3T;
+
+    CrossProduct(int _rows, int _cols, int _windowSize, const Mat& _K) :
+        RgbdNormalsImpl<DataType>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT)
+    { }
+
+    /** Compute cached data
+     */
+    virtual void cache() const CV_OVERRIDE
+    {
+        this->cacheIsDirty = false;
+    }
+
+    static inline Point3T fromVec(Vec4T v)
+    {
+        return {v[0], v[1], v[2]};
+    }
+
+    static inline Vec4T toVec4(Point3T p)
+    {
+        return {p.x, p.y, p.z, 0};
+    }
+
+    static inline bool haveNaNs(Point3T p)
+    {
+        return cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z);
+    }
+
+    /** Compute the normals
+     * @param points reprojected depth points
+     * @param normals generated normals
+     * @return
+     */
+    virtual void compute(const Mat& points, Mat& normals) const CV_OVERRIDE
+    {
+        for(int y = 0; y < this->rows; y++)
+        {
+            const Vec4T* ptsRow0 = points.ptr<Vec4T>(y);
+            const Vec4T* ptsRow1 = (y < this->rows - 1) ? points.ptr<Vec4T>(y + 1) : nullptr;
+            Vec4T *normRow = normals.ptr<Vec4T>(y);
+
+            for (int x = 0; x < this->cols; x++)
+            {
+                Point3T v00 = fromVec(ptsRow0[x]);
+                const float qnan = std::numeric_limits<float>::quiet_NaN();
+                Point3T n(qnan, qnan, qnan);
+
+                if ((x < this->cols - 1) && (y < this->rows - 1) && !haveNaNs(v00))
+                {
+                    Point3T v01 = fromVec(ptsRow0[x + 1]);
+                    Point3T v10 = fromVec(ptsRow1[x]);
+
+                    if (!haveNaNs(v01) && !haveNaNs(v10))
+                    {
+                        Vec3T vec = (v10 - v00).cross(v01 - v00);
+                        n = normalize(vec);
+                    }
+                }
+
+                normRow[x] = toVec4(n);
+            }
+        }
+    }
+};
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K, int windowSize, float diffThreshold, RgbdNormalsMethod method)
+{
+    CV_Assert(rows > 0 && cols > 0 && (depth == CV_32F || depth == CV_64F));
+    CV_Assert(windowSize == 1 || windowSize == 3 || windowSize == 5 || windowSize == 7);
+    CV_Assert(K.cols() == 3 && K.rows() == 3 && (K.depth() == CV_32F || K.depth() == CV_64F));
+
+    Mat mK = K.getMat();
+    Ptr<RgbdNormals> ptr;
+    switch (method)
+    {
+    case (RGBD_NORMALS_METHOD_FALS):
+    {
+        if (depth == CV_32F)
+            ptr = makePtr<FALS<float> >(rows, cols, windowSize, mK);
+        else
+            ptr = makePtr<FALS<double>>(rows, cols, windowSize, mK);
+        break;
+    }
+    case (RGBD_NORMALS_METHOD_LINEMOD):
+    {
+        CV_Assert(diffThreshold > 0);
+        if (depth == CV_32F)
+            ptr = makePtr<LINEMOD<float> >(rows, cols, windowSize, mK, diffThreshold);
+        else
+            ptr = makePtr<LINEMOD<double>>(rows, cols, windowSize, mK, diffThreshold);
+        break;
+    }
+    case RGBD_NORMALS_METHOD_SRI:
+    {
+        if (depth == CV_32F)
+            ptr = makePtr<SRI<float> >(rows, cols, windowSize, mK);
+        else
+            ptr = makePtr<SRI<double>>(rows, cols, windowSize, mK);
+        break;
+    }
+    case RGBD_NORMALS_METHOD_CROSS_PRODUCT:
+    {
+        if (depth == CV_32F)
+            ptr = makePtr<CrossProduct<float> >(rows, cols, windowSize, mK);
+        else
+            ptr = makePtr<CrossProduct<double>>(rows, cols, windowSize, mK);
+        break;
+    }
+    default:
+        CV_Error(Error::StsBadArg, "Unknown normals compute algorithm");
+    }
+
+    return ptr;
+}
+
+} // namespace cv

modules/ptcloud/src/octree.cpp

@@ -0,0 +1,695 @@
+// 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
+
+#include "precomp.hpp"
+#include "octree.hpp"
+#include "opencv2/geometry/3d.hpp"
+
+namespace cv{
+
+OctreeNode::OctreeNode() :
+    children(),
+    depth(0),
+    size(0),
+    origin(0,0,0),
+    neigh(),
+    parentIndex(-1)
+{ }
+
+OctreeNode::OctreeNode(int _depth, double _size, const Point3f &_origin, int _parentIndex) :
+    children(),
+    depth(_depth),
+    size(_size),
+    origin(_origin),
+    neigh(),
+    parentIndex(_parentIndex)
+{ }
+
+bool OctreeNode::empty() const
+{
+    if(this->isLeaf)
+    {
+        if(this->pointList.empty())
+            return true;
+        else
+            return false;
+    }
+    else
+    {
+        for(size_t i = 0; i < 8; i++)
+        {
+            if(!this->children[i].empty())
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+}
+
+
+bool OctreeNode::isPointInBound(const Point3f& _point) const
+{
+    Point3f eps;
+    eps.x = std::max(std::abs(_point.x), std::abs(this->origin.x));
+    eps.y = std::max(std::abs(_point.y), std::abs(this->origin.y));
+    eps.z = std::max(std::abs(_point.z), std::abs(this->origin.z));
+    eps *= std::numeric_limits<float>::epsilon();
+    Point3f ptEps = _point + eps;
+    Point3f upPt = this->origin + eps + Point3f {(float)this->size, (float)this->size, (float)this->size};
+
+    return (ptEps.x >= this->origin.x) &&
+           (ptEps.y >= this->origin.y) &&
+           (ptEps.z >= this->origin.z) &&
+           (_point.x <= upPt.x) &&
+           (_point.y <= upPt.y) &&
+           (_point.z <= upPt.z);
+}
+
+struct Octree::Impl
+{
+public:
+    Impl() : Impl(0, 0, {0, 0, 0}, 0, false) { }
+
+    Impl(int _maxDepth, double _size, const Point3f& _origin, double _resolution,
+         bool _hasColor) :
+        maxDepth(_maxDepth),
+        size(_size),
+        origin(_origin),
+        resolution(_resolution),
+        hasColor(_hasColor)
+    { }
+
+    ~Impl() { }
+
+    void fill(bool useResolution, InputArray pointCloud, InputArray colorAttribute);
+    bool insertPoint(const Point3f& point, const Point3f &color);
+
+    // The pointer to Octree root node
+    Ptr <OctreeNode> rootNode = nullptr;
+    //! Max depth of the Octree
+    int maxDepth;
+    //! The size of the cube
+    double size;
+    //! The origin coordinate of root node
+    Point3f origin;
+    //! The size of the leaf node
+    double resolution;
+    //! Whether the point cloud has a color attribute
+    bool hasColor;
+};
+
+Octree::Octree() :
+    p(makePtr<Impl>())
+{ }
+
+Ptr<Octree> Octree::createWithDepth(int maxDepth, double size, const Point3f& origin, bool withColors)
+{
+    CV_Assert(maxDepth > 0);
+    CV_Assert(size > 0);
+
+    Ptr<Octree> octree = makePtr<Octree>();
+    octree->p = makePtr<Impl>(maxDepth, size, origin, /*resolution*/ 0, withColors);
+    return octree;
+}
+
+Ptr<Octree> Octree::createWithDepth(int maxDepth, InputArray pointCloud, InputArray colors)
+{
+    CV_Assert(maxDepth > 0);
+
+    Ptr<Octree> octree = makePtr<Octree>();
+    octree->p->maxDepth = maxDepth;
+    octree->p->fill(/* useResolution */ false, pointCloud, colors);
+    return octree;
+}
+
+Ptr<Octree> Octree::createWithResolution(double resolution, double size, const Point3f& origin, bool withColors)
+{
+    CV_Assert(resolution > 0);
+    CV_Assert(size > 0);
+
+    Ptr<Octree> octree = makePtr<Octree>();
+    octree->p = makePtr<Impl>(/*maxDepth*/ 0, size, origin, resolution, withColors);
+    return octree;
+}
+
+Ptr<Octree> Octree::createWithResolution(double resolution, InputArray pointCloud, InputArray colors)
+{
+    CV_Assert(resolution > 0);
+
+    Ptr<Octree> octree = makePtr<Octree>();
+    octree->p->resolution = resolution;
+    octree->p->fill(/* useResolution */ true, pointCloud, colors);
+    return octree;
+}
+
+Octree::~Octree() { }
+
+bool Octree::insertPoint(const Point3f& point, const Point3f &color)
+{
+    return p->insertPoint(point, color);
+}
+
+bool Octree::Impl::insertPoint(const Point3f& point, const Point3f &color)
+{
+    size_t depthMask = (size_t)(1ULL << (this->maxDepth - 1));
+
+    if(this->rootNode.empty())
+    {
+        this->rootNode = new OctreeNode( 0, this->size, this->origin, -1);
+    }
+
+    bool pointInBoundFlag = this->rootNode->isPointInBound(point);
+    if(this->rootNode->depth == 0 && !pointInBoundFlag)
+    {
+        return false;
+    }
+
+    OctreeKey key((size_t)floor((point.x - this->origin.x) / this->resolution),
+                  (size_t)floor((point.y - this->origin.y) / this->resolution),
+                  (size_t)floor((point.z - this->origin.z) / this->resolution));
+
+    Ptr<OctreeNode> node = this->rootNode;
+    while (node->depth != maxDepth)
+    {
+        double childSize = node->size * 0.5;
+
+        // calculate the index and the origin of child.
+        size_t childIndex = key.findChildIdxByMask(depthMask);
+        size_t xIndex = (childIndex & 1) ? 1 : 0;
+        size_t yIndex = (childIndex & 2) ? 1 : 0;
+        size_t zIndex = (childIndex & 4) ? 1 : 0;
+
+        Point3f childOrigin = node->origin + Point3f(float(xIndex), float(yIndex), float(zIndex)) * float(childSize);
+
+        Ptr<OctreeNode> &childPtr = node->children[childIndex];
+        if (!childPtr)
+        {
+            childPtr = new OctreeNode(node->depth + 1, childSize, childOrigin, int(childIndex));
+            childPtr->parent = node;
+        }
+
+        node = childPtr;
+        depthMask = depthMask >> 1;
+    }
+
+    node->isLeaf = true;
+    node->pointList.push_back(point);
+    node->colorList.push_back(color);
+
+    return true;
+}
+
+
+static Vec6f getBoundingBox(const Mat& points)
+{
+    const float mval = std::numeric_limits<float>::max();
+    Vec6f bb(mval, mval, mval, -mval, -mval, -mval);
+
+    for (int i = 0; i < (int)points.total(); i++)
+    {
+        Point3f pt = points.at<Point3f>(i);
+        bb[0] = min(bb[0], pt.x);
+        bb[1] = min(bb[1], pt.y);
+        bb[2] = min(bb[2], pt.z);
+        bb[3] = max(bb[3], pt.x);
+        bb[4] = max(bb[4], pt.y);
+        bb[5] = max(bb[5], pt.z);
+    }
+
+    return bb;
+}
+
+void Octree::Impl::fill(bool useResolution, InputArray _points, InputArray _colors)
+{
+    CV_CheckFalse(_points.empty(), "No points provided");
+
+    Mat points, colors;
+    int nPoints = 0, nColors = 0;
+
+    int pointType = _points.type();
+    CV_Assert(pointType == CV_32FC1 || pointType == CV_32FC3);
+    points = _points.getMat();
+    // transform 3xN matrix to Nx3, except 3x3
+    if ((_points.channels() == 1) && (_points.rows() == 3) && (_points.cols() != 3))
+    {
+        points = points.t();
+    }
+    // This transposition is performed on 1xN matrix so it's almost free in terms of performance
+    points = points.reshape(3, 1).t();
+    nPoints = (int)points.total();
+
+    if (!_colors.empty())
+    {
+        int colorType = _colors.type();
+        CV_Assert(colorType == CV_32FC1 || colorType == CV_32FC3);
+        colors = _colors.getMat();
+        // transform 3xN matrix to Nx3, except 3x3
+        if ((_colors.channels() == 1) && (_colors.rows() == 3) && (_colors.cols() != 3))
+        {
+            colors = colors.t();
+        }
+        colors = colors.reshape(3, 1).t();
+        nColors = (int)colors.total();
+
+        CV_Assert(nColors == nPoints);
+        this->hasColor = true;
+    }
+
+    Vec6f bbox = getBoundingBox(points);
+    Point3f minBound(bbox[0], bbox[1], bbox[2]);
+    Point3f maxBound(bbox[3], bbox[4], bbox[5]);
+
+    double maxSize = max(max(maxBound.x - minBound.x, maxBound.y - minBound.y), maxBound.z - minBound.z);
+
+    // Extend maxSize to the closest power of 2 that exceeds it for bit operations
+    maxSize = double(1 << int(ceil(log2(maxSize))));
+
+    // to calculate maxDepth from resolution or vice versa
+    if (useResolution)
+    {
+        this->maxDepth = (int)ceil(log2(maxSize / this->resolution));
+    }
+    else
+    {
+        this->resolution = (maxSize / (1 << (this->maxDepth + 1)));
+    }
+
+    this->size = (1 << this->maxDepth) * this->resolution;
+    this->origin = Point3f(float(floor(minBound.x / this->resolution) * this->resolution),
+                           float(floor(minBound.y / this->resolution) * this->resolution),
+                           float(floor(minBound.z / this->resolution) * this->resolution));
+
+    // Insert every point in PointCloud data.
+    for (int idx = 0; idx < nPoints; idx++)
+    {
+        Point3f pt = points.at<Point3f>(idx);
+        Point3f insertColor = this->hasColor ? colors.at<Point3f>(idx) : Point3f { };
+        if (!this->insertPoint(pt, insertColor))
+        {
+            CV_Error(Error::StsBadArg, "The point is out of boundary!");
+        }
+    }
+}
+
+
+void Octree::clear()
+{
+    p = makePtr<Impl>();
+}
+
+bool Octree::empty() const
+{
+    return p->rootNode.empty();
+}
+
+
+bool Octree::isPointInBound(const Point3f& _point) const
+{
+    return p->rootNode->isPointInBound(_point);
+}
+
+bool Octree::deletePoint(const Point3f& point)
+{
+    OctreeKey key = OctreeKey((size_t)floor((point.x - this->p->origin.x) / p->resolution),
+                              (size_t)floor((point.y - this->p->origin.y) / p->resolution),
+                              (size_t)floor((point.z - this->p->origin.z) / p->resolution));
+    size_t depthMask = (size_t)1 << (p->maxDepth - 1);
+
+    Ptr<OctreeNode> node = p->rootNode;
+    while(node)
+    {
+        if (node->empty())
+        {
+            node = nullptr;
+        }
+        else if (node->isLeaf)
+        {
+            const float eps = 1e-9f;
+            bool found = std::any_of(node->pointList.begin(), node->pointList.end(),
+                [point, eps](const Point3f& pt) -> bool
+                {
+                    return abs(point.x - pt.x) < eps &&
+                           abs(point.y - pt.y) < eps &&
+                           abs(point.z - pt.z) < eps;
+                });
+            if (!found)
+                node = nullptr;
+            break;
+        }
+        else
+        {
+            node = node->children[key.findChildIdxByMask(depthMask)];
+            depthMask = depthMask >> 1;
+        }
+    }
+
+    if(!node)
+        return false;
+
+    const float eps = 1e-9f;
+
+    // we've found a leaf node and delete all verts equal to given one
+    size_t ctr = 0;
+    while (!node->pointList.empty() && ctr < node->pointList.size())
+    {
+        if (abs(point.x - node->pointList[ctr].x) < eps &&
+            abs(point.y - node->pointList[ctr].y) < eps &&
+            abs(point.z - node->pointList[ctr].z) < eps)
+        {
+            node->pointList.erase(node->pointList.begin() + ctr);
+        }
+        else
+        {
+            ctr++;
+        }
+    }
+
+    if (node->pointList.empty())
+    {
+        // empty node and its empty parents should be removed
+        OctreeNode *parentPtr = node->parent;
+        int parentdIdx = node->parentIndex;
+
+        while (parentPtr)
+        {
+            parentPtr->children[parentdIdx].release();
+
+            // check if all children were deleted
+            bool deleteFlag = true;
+            for (size_t i = 0; i < 8; i++)
+            {
+                if (!parentPtr->children[i].empty())
+                {
+                    deleteFlag = false;
+                    break;
+                }
+            }
+
+            if (deleteFlag)
+            {
+                // we're at empty node, going up
+                parentdIdx = parentPtr->parentIndex;
+                parentPtr = parentPtr->parent;
+            }
+            else
+            {
+                // reached first non-empty node, stopping
+                parentPtr = nullptr;
+            }
+        }
+    }
+    return true;
+}
+
+
+void Octree::getPointCloudByOctree(OutputArray restorePointCloud, OutputArray restoreColor)
+{
+    Ptr<OctreeNode> root = p->rootNode;
+    double resolution = p->resolution;
+    std::vector<Point3f> outPts, outColors;
+
+    typedef std::tuple<Ptr<OctreeNode>, size_t, size_t, size_t> stack_element;
+    std::stack<stack_element> toCheck;
+    toCheck.push(stack_element(root, 0, 0, 0));
+    while (!toCheck.empty())
+    {
+        auto top = toCheck.top();
+        toCheck.pop();
+        Ptr<OctreeNode> node = std::get<0>(top);
+        size_t x_key = std::get<1>(top);
+        size_t y_key = std::get<2>(top);
+        size_t z_key = std::get<3>(top);
+
+        if (node->isLeaf)
+        {
+            outPts.emplace_back(
+                    (float) (resolution * x_key) + (float) (resolution * 0.5) + p->origin.x,
+                    (float) (resolution * y_key) + (float) (resolution * 0.5) + p->origin.y,
+                    (float) (resolution * z_key) + (float) (resolution * 0.5) + p->origin.z);
+            if (p->hasColor)
+            {
+                Point3f avgColor { };
+                for (const auto& c : node->colorList)
+                {
+                    avgColor += c;
+                }
+                avgColor *= (1.f/(float)node->colorList.size());
+                outColors.emplace_back(avgColor);
+            }
+        }
+        else
+        {
+            unsigned char x_mask = 1;
+            unsigned char y_mask = 2;
+            unsigned char z_mask = 4;
+            for (unsigned char i = 0; i < 8; i++)
+            {
+                size_t x_copy = x_key;
+                size_t y_copy = y_key;
+                size_t z_copy = z_key;
+                if (!node->children[i].empty())
+                {
+                    size_t x_offSet = !!(x_mask & i);
+                    size_t y_offSet = !!(y_mask & i);
+                    size_t z_offSet = !!(z_mask & i);
+                    x_copy = (x_copy << 1) | x_offSet;
+                    y_copy = (y_copy << 1) | y_offSet;
+                    z_copy = (z_copy << 1) | z_offSet;
+                    toCheck.push(stack_element(node->children[i], x_copy, y_copy, z_copy));
+                }
+            }
+        }
+    }
+
+    if (restorePointCloud.needed())
+    {
+        Mat(outPts).copyTo(restorePointCloud);
+    }
+    if (restoreColor.needed())
+    {
+        Mat(outColors).copyTo(restoreColor);
+    }
+}
+
+
+static float SquaredDistance(const Point3f& query, const Point3f& origin)
+{
+    Point3f diff = query - origin;
+    return diff.dot(diff);
+}
+
+bool OctreeNode::overlap(const Point3f& query, float squareRadius) const
+{
+    float halfSize = float(this->size * 0.5);
+    Point3f center = this->origin + Point3f( halfSize, halfSize, halfSize );
+
+    float dist = SquaredDistance(center, query);
+    float temp = float(this->size) * float(this->size) * 3.0f;
+
+    return ( dist + dist * std::numeric_limits<float>::epsilon() ) <= float(temp * 0.25f + squareRadius + sqrt(temp * squareRadius)) ;
+}
+
+
+int Octree::radiusNNSearch(const Point3f& query, float radius, OutputArray pointSet, OutputArray squareDistSet) const
+{
+    return this->radiusNNSearch(query, radius, pointSet, noArray(), squareDistSet);
+}
+
+int Octree::radiusNNSearch(const Point3f& query, float radius, OutputArray points, OutputArray colors, OutputArray squareDists) const
+{
+    std::vector<Point3f> outPoints, outColors;
+    std::vector<float> outSqDists;
+
+    if (!p->rootNode.empty())
+    {
+        float squareRadius = radius * radius;
+
+        std::vector<std::tuple<float, Point3f, Point3f>> candidatePoints;
+
+        std::stack<Ptr<OctreeNode>> toCheck;
+        toCheck.push(p->rootNode);
+
+        while (!toCheck.empty())
+        {
+            Ptr<OctreeNode> node = toCheck.top();
+            toCheck.pop();
+            for(size_t i = 0; i < 8; i++)
+            {
+                Ptr<OctreeNode> child = node->children[i];
+                if( child && child->overlap(query, squareRadius))
+                {
+                    if(child->isLeaf)
+                    {
+                        for(size_t j = 0; j < child->pointList.size(); j++)
+                        {
+                            Point3f pt = child->pointList[j];
+                            Point3f col;
+                            if (!child->colorList.empty())
+                            {
+                                col = child->colorList[j];
+                            }
+                            float dist = SquaredDistance(pt, query);
+                            if(dist + dist * std::numeric_limits<float>::epsilon() <= squareRadius)
+                            {
+                                candidatePoints.emplace_back(dist, pt, col);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        toCheck.push(child);
+                    }
+                }
+            }
+        }
+
+        for (size_t i = 0; i < candidatePoints.size(); i++)
+        {
+            auto cp = candidatePoints[i];
+            outSqDists.push_back(std::get<0>(cp));
+            outPoints.push_back(std::get<1>(cp));
+            outColors.push_back(std::get<2>(cp));
+        }
+    }
+
+    if (points.needed())
+    {
+        Mat(outPoints).copyTo(points);
+    }
+    if (colors.needed())
+    {
+        CV_Assert(this->p->hasColor);
+        Mat(outColors).copyTo(colors);
+    }
+    if (squareDists.needed())
+    {
+        Mat(outSqDists).copyTo(squareDists);
+    }
+
+    return int(outPoints.size());
+}
+
+
+void OctreeNode::KNNSearchRecurse(const Point3f& query, const int K,
+                                  float& smallestDist, std::vector<std::tuple<float, Point3f, Point3f>>& candidatePoint) const
+{
+    std::vector<std::pair<float, int>> priorityQue;
+
+    // Add the non-empty OctreeNode to priorityQue
+    for(size_t i = 0; i < 8; i++)
+    {
+        Ptr<OctreeNode> child = this->children[i];
+        if(child)
+        {
+            float halfSize = float(child->size * 0.5);
+
+            Point3f center = child->origin + Point3f(halfSize, halfSize, halfSize);
+
+            float dist = SquaredDistance(query, center);
+            priorityQue.emplace_back(dist, int(i));
+        }
+    }
+
+    std::sort(priorityQue.rbegin(), priorityQue.rend(),
+        [](const std::pair<float, int>& a, const std::pair<float, int>& b) -> bool
+        {
+            return std::get<0>(a) < std::get<0>(b);
+        });
+    Ptr<OctreeNode> child = this->children[std::get<1>(priorityQue.back())];
+
+    while (!priorityQue.empty() && child->overlap(query, smallestDist))
+    {
+        if (!child->isLeaf)
+        {
+            child->KNNSearchRecurse(query, K, smallestDist, candidatePoint);
+        }
+        else
+        {
+            for (size_t i = 0; i < child->pointList.size(); i++)
+            {
+                float dist = SquaredDistance(child->pointList[i], query);
+
+                if ( dist + dist * std::numeric_limits<float>::epsilon() <= smallestDist )
+                {
+                    Point3f pt = child->pointList[i];
+                    Point3f col { };
+                    if (child->colorList.empty())
+                    {
+                        col = child->colorList[i];
+                    }
+                    candidatePoint.emplace_back(dist, pt, col);
+                }
+            }
+
+            std::sort(candidatePoint.begin(), candidatePoint.end(),
+                [](const std::tuple<float, Point3f, Point3f>& a, const std::tuple<float, Point3f, Point3f>& b) -> bool
+                {
+                    return std::get<0>(a) < std::get<0>(b);
+                }
+            );
+
+            if (int(candidatePoint.size()) > K)
+            {
+                candidatePoint.resize(K);
+            }
+
+            if (int(candidatePoint.size()) == K)
+            {
+                smallestDist = std::get<0>(candidatePoint.back());
+            }
+        }
+
+        priorityQue.pop_back();
+
+        // To next child
+        if(!priorityQue.empty())
+        {
+            child = this->children[std::get<1>(priorityQue.back())];
+        }
+    }
+}
+
+
+void Octree::KNNSearch(const Point3f &query, const int K, OutputArray pointSet, OutputArray squareDistSet) const
+{
+    this->KNNSearch(query, K, pointSet, noArray(), squareDistSet);
+}
+
+void Octree::KNNSearch(const Point3f &query, const int K, OutputArray points, OutputArray colors, OutputArray squareDists) const
+{
+    std::vector<Point3f> outPoints, outColors;
+    std::vector<float> outSqDists;
+
+    if (!p->rootNode.empty())
+    {
+        std::vector<std::tuple<float, Point3f, Point3f>> candidatePoints;
+        float smallestDist = std::numeric_limits<float>::max();
+
+        p->rootNode->KNNSearchRecurse(query, K, smallestDist, candidatePoints);
+
+        for(size_t i = 0; i < candidatePoints.size(); i++)
+        {
+            auto cp = candidatePoints[i];
+            outSqDists.push_back(std::get<0>(cp));
+            outPoints.push_back(std::get<1>(cp));
+            outColors.push_back(std::get<2>(cp));
+        }
+    }
+
+    if (points.needed())
+    {
+        Mat(outPoints).copyTo(points);
+    }
+    if (colors.needed())
+    {
+        CV_Assert(this->p->hasColor);
+        Mat(outColors).copyTo(colors);
+    }
+    if (squareDists.needed())
+    {
+        Mat(outSqDists).copyTo(squareDists);
+    }
+}
+}

modules/ptcloud/src/octree.hpp

@@ -0,0 +1,181 @@
+// 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.
+//
+// Copyright (C) 2021, Huawei Technologies Co., Ltd. All rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Author: Zihao Mu <zihaomu6@gmail.com>
+//         Liangqian Kong <chargerKong@126.com>
+//         Longbu Wang <riskiest@gmail.com>
+
+#ifndef OPENCV_3D_SRC_OCTREE_HPP
+#define OPENCV_3D_SRC_OCTREE_HPP
+
+#include <vector>
+#include <array>
+#include "opencv2/core.hpp"
+
+namespace cv
+{
+// Forward declaration
+class OctreeKey;
+
+/** @brief OctreeNode for Octree.
+
+The class OctreeNode represents the node of the octree. Each node contains 8 children, which are used to divide the
+space cube into eight parts. Each octree node represents a cube.
+And these eight children will have a fixed order, the order is described as follows:
+
+For illustration, assume,
+    rootNode: origin == (0, 0, 0), size == 2
+ Then,
+    children[0]: origin == (0, 0, 0), size == 1
+    children[1]: origin == (1, 0, 0), size == 1, along X-axis next to child 0
+    children[2]: origin == (0, 1, 0), size == 1, along Y-axis next to child 0
+    children[3]: origin == (1, 1, 0), size == 1, in X-Y plane
+    children[4]: origin == (0, 0, 1), size == 1, along Z-axis next to child 0
+    children[5]: origin == (1, 0, 1), size == 1, in X-Z plane
+    children[6]: origin == (0, 1, 1), size == 1, in Y-Z plane
+    children[7]: origin == (1, 1, 1), size == 1, furthest from child 0
+
+There are two kinds of nodes in an octree, intermediate nodes and leaf nodes, which are distinguished by isLeaf.
+Intermediate nodes are used to contain leaf nodes, and leaf nodes will contain pointers to all pointcloud data
+within the node, which will be used for octree indexing and mapping from point clouds to octree. Note that,
+in an octree, each leaf node contains at least one point cloud data. Similarly, every intermediate OctreeNode
+contains at least one non-empty child pointer, except for the root node.
+*/
+class OctreeNode
+{
+public:
+
+    /**
+    * There are multiple constructors to create OctreeNode.
+    * */
+    OctreeNode();
+
+    /** @overload
+    *
+    * @param _depth The depth of the current node. The depth of the root node is 0, and the leaf node is equal
+    * to the depth of Octree.
+    * @param _size The length of the OctreeNode. In space, every OctreeNode represents a cube.
+    * @param _origin The absolute coordinates of the center of the cube.
+    * @param _parentIndex The serial number of the child of the current node in the parent node,
+    * the range is (-1~7). Among them, only the root node's _parentIndex is -1.
+    */
+    OctreeNode(int _depth, double _size, const Point3f& _origin, int _parentIndex);
+
+    //! returns true if the rootNode is NULL.
+    bool empty() const;
+
+    bool isPointInBound(const Point3f& _point) const;
+
+    bool overlap(const Point3f& query, float squareRadius) const;
+
+    void KNNSearchRecurse(const Point3f& query, const int K, float& smallestDist, std::vector<std::tuple<float, Point3f, Point3f>>& candidatePoint) const;
+
+    //! Contains 8 pointers to its 8 children.
+    std::array<Ptr<OctreeNode>, 8> children;
+
+    //! Point to the parent node of the current node. The root node has no parent node and the value is NULL.
+    OctreeNode* parent;
+
+    //! The depth of the current node. The depth of the root node is 0, and the leaf node is equal to the depth of Octree.
+    int depth;
+
+    //! The length of the OctreeNode. In space, every OctreeNode represents a cube.
+    double size;
+
+    //! Absolute coordinates of the smallest point of the cube.
+    //! And the center of cube is `center = origin + Point3f(size/2, size/2, size/2)`.
+    Point3f origin;
+
+    //! RAHTCoefficient of octree node, used for color attribute compression.
+    Point3f RAHTCoefficient = { };
+
+    /**  The list of 6 adjacent neighbor node.
+        *    index mapping:
+        *     +z                        [101]
+        *      |                          |    [110]
+        *      |                          |  /
+        *      O-------- +x    [001]----{000} ----[011]
+        *     /                       /   |
+        *    /                   [010]    |
+        *  +y                           [100]
+        *  index 000, 111 are reserved
+        */
+    std::array<Ptr<OctreeNode>, 8> neigh;
+
+    /**  The serial number of the child of the current node in the parent node,
+    * the range is (-1~7). Among them, only the root node's _parentIndex is -1.
+    */
+    int parentIndex;
+
+    //! If the OctreeNode is LeafNode.
+    bool isLeaf = false;
+
+    //! Contains pointers to all point cloud data in this node.
+    std::vector<Point3f> pointList;
+
+    //! color attribute of octree node.
+    std::vector<Point3f> colorList;
+};
+
+/** @brief Key for pointCloud, used to compute the child node index through bit operations.
+
+When building the octree, the point cloud data is firstly voxelized/discretized: by inserting
+all the points into a voxel coordinate system. For example, when resolution is set to 0.01, a point
+with coordinate Point3f(0.251,0.502,0.753) would be transformed to:(0.251/0.01,0.502/0.01,0.753/0.01)
+=(25,50,75). And the OctreeKey will be (x_key:1_1001,y_key:11_0010,z_key:100_1011). Assume the Octree->depth
+is 100_0000, It can quickly calculate the index of the child nodes at each layer.
+layer    Depth Mask   x&Depth Mask    y&Depth Mask    z&Depth Mask    Child Index(0-7)
+1        100_0000     0               0               1               4
+2        10_0000      0               1               0               2
+3        1_0000       1               1               0               3
+4        1000         1               0               1               5
+5        100          0               0               0               0
+6        10           0               1               1               6
+7        1            1               0               1               5
+*/
+
+class OctreeKey
+{
+public:
+    size_t x_key;
+    size_t y_key;
+    size_t z_key;
+
+public:
+    OctreeKey() : x_key(0), y_key(0), z_key(0) { }
+    OctreeKey(size_t x, size_t y, size_t z) : x_key(x), y_key(y), z_key(z) { }
+
+    /** @brief compute the child node index through bit operations.
+    *
+    * @param mask The mask of specify layer.
+    * @return the index of child(0-7)
+    */
+    inline unsigned char findChildIdxByMask(size_t mask) const
+    {
+        return static_cast<unsigned char>((!!(z_key & mask))<<2) | ((!!(y_key & mask))<<1) | (!!(x_key & mask));
+    }
+
+    /** @brief get occupancy code from node.
+    *
+    * The occupancy code type is unsigned char that represents whether the eight child nodes of the octree node exist
+    * If a octree node has 3 child which indexes are 0,1,7, then the occupancy code of this node is 1000_0011
+    * @param node The octree node.
+    * @return the occupancy code(0000_0000-1111_1111)
+    */
+    static inline unsigned char getBitPattern(OctreeNode &node)
+    {
+        unsigned char res = 0;
+        for (unsigned char i = 0; i < node.children.size(); i++)
+        {
+            res |= static_cast<unsigned char>((!node.children[i].empty()) << i);
+        }
+        return res;
+    }
+};
+
+}
+#endif //OPENCV_3D_SRC_OCTREE_HPP

modules/ptcloud/src/odometry.cpp

@@ -0,0 +1,529 @@
+// 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
+
+#include "precomp.hpp"
+#include "utils.hpp"
+#include "opencv2/ptcloud/odometry.hpp"
+#include "odometry_functions.hpp"
+
+namespace cv
+{
+
+class Odometry::Impl
+{
+public:
+    Impl() {};
+    virtual ~Impl() {};
+    virtual void prepareFrame(OdometryFrame& frame) const = 0;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const = 0;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const = 0;
+    virtual bool compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const = 0;
+    virtual bool compute(InputArray srcDepth, InputArray srcRGB,
+                         InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const = 0;
+    virtual Ptr<RgbdNormals> getNormalsComputer() const = 0;
+};
+
+
+class OdometryICP : public Odometry::Impl
+{
+private:
+    OdometrySettings settings;
+    OdometryAlgoType algtype;
+    mutable Ptr<RgbdNormals> normalsComputer;
+
+public:
+    OdometryICP(OdometrySettings _settings, OdometryAlgoType _algtype) :
+        settings(_settings), algtype(_algtype), normalsComputer()
+    { }
+    ~OdometryICP() { }
+
+    virtual void prepareFrame(OdometryFrame& frame) const override;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const override;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
+    virtual bool compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const override;
+    virtual bool compute(InputArray srcDepth, InputArray srcRGB,
+                         InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const override;
+    virtual Ptr<RgbdNormals> getNormalsComputer() const override;
+};
+
+Ptr<RgbdNormals> OdometryICP::getNormalsComputer() const
+{
+    return this->normalsComputer;
+}
+
+void OdometryICP::prepareFrame(OdometryFrame& frame) const
+{
+    prepareICPFrame(frame, frame, this->normalsComputer, this->settings, this->algtype);
+}
+
+void OdometryICP::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const
+{
+    prepareICPFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype);
+}
+
+bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
+{
+    Matx33f cameraMatrix;
+    settings.getCameraMatrix(cameraMatrix);
+    std::vector<int> iterCounts;
+    settings.getIterCounts(iterCounts);
+    bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix,
+                                         this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
+                                         iterCounts, this->settings.getMaxTranslation(),
+                                         this->settings.getMaxRotation(), settings.getSobelScale(),
+                                         OdometryType::DEPTH, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype);
+    return isCorrect;
+}
+
+bool OdometryICP::compute(InputArray _srcDepth, InputArray _dstDepth, OutputArray Rt) const
+{
+    OdometryFrame srcFrame(_srcDepth);
+    OdometryFrame dstFrame(_dstDepth);
+
+    prepareICPFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype);
+
+    bool isCorrect = compute(srcFrame, dstFrame, Rt);
+    return isCorrect;
+}
+
+bool OdometryICP::compute(InputArray srcDepth, InputArray srcRGB,
+                          InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const
+{
+    CV_UNUSED(srcDepth);
+    CV_UNUSED(srcRGB);
+    CV_UNUSED(dstDepth);
+    CV_UNUSED(dstRGB);
+    CV_UNUSED(Rt);
+    CV_Error(cv::Error::StsBadFunc, "This odometry does not work with rgb data");
+}
+
+class OdometryRGB : public Odometry::Impl
+{
+private:
+    OdometrySettings settings;
+    OdometryAlgoType algtype;
+
+public:
+    OdometryRGB(OdometrySettings _settings, OdometryAlgoType _algtype) : settings(_settings), algtype(_algtype) { }
+    ~OdometryRGB() { }
+
+    virtual void prepareFrame(OdometryFrame& frame) const override;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const override;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
+    virtual bool compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const override;
+    virtual bool compute(InputArray srcDepth, InputArray srcRGB,
+                         InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const override;
+    virtual Ptr<RgbdNormals> getNormalsComputer() const override { return Ptr<RgbdNormals>(); }
+};
+
+
+void OdometryRGB::prepareFrame(OdometryFrame& frame) const
+{
+    prepareRGBFrame(frame, frame, this->settings);
+}
+
+void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const
+{
+    prepareRGBFrame(srcFrame, dstFrame, this->settings);
+}
+
+bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
+{
+    Matx33f cameraMatrix;
+    settings.getCameraMatrix(cameraMatrix);
+    std::vector<int> iterCounts;
+    settings.getIterCounts(iterCounts);
+    bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix,
+                                         this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
+                                         iterCounts, this->settings.getMaxTranslation(),
+                                         this->settings.getMaxRotation(), settings.getSobelScale(),
+                                         OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype);
+    return isCorrect;
+}
+
+bool OdometryRGB::compute(InputArray _srcDepth, InputArray _dstDepth, OutputArray Rt) const
+{
+    CV_UNUSED(_srcDepth);
+    CV_UNUSED(_dstDepth);
+    CV_UNUSED(Rt);
+    CV_Error(cv::Error::StsBadFunc, "This odometry algorithm requires depth and rgb data simultaneously");
+}
+
+bool OdometryRGB::compute(InputArray srcDepth, InputArray srcRGB, InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const
+{
+    OdometryFrame srcFrame(srcDepth, srcRGB);
+    OdometryFrame dstFrame(dstDepth, dstRGB);
+
+    prepareRGBFrame(srcFrame, dstFrame, this->settings);
+
+    return compute(srcFrame, dstFrame, Rt);
+}
+
+class OdometryRGBD : public Odometry::Impl
+{
+private:
+    OdometrySettings settings;
+    OdometryAlgoType algtype;
+    mutable Ptr<RgbdNormals> normalsComputer;
+
+public:
+    OdometryRGBD(OdometrySettings _settings, OdometryAlgoType _algtype) : settings(_settings), algtype(_algtype), normalsComputer() { }
+    ~OdometryRGBD() { }
+
+    virtual void prepareFrame(OdometryFrame& frame) const override;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const override;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
+    virtual bool compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const override;
+    virtual bool compute(InputArray srcDepth, InputArray srcRGB,
+                         InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const override;
+    virtual Ptr<RgbdNormals> getNormalsComputer() const override;
+};
+
+Ptr<RgbdNormals> OdometryRGBD::getNormalsComputer() const
+{
+    return normalsComputer;
+}
+
+void OdometryRGBD::prepareFrame(OdometryFrame& frame) const
+{
+    prepareRGBDFrame(frame, frame, this->normalsComputer, this->settings, this->algtype);
+}
+
+void OdometryRGBD::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const
+{
+    prepareRGBDFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype);
+}
+
+bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
+{
+    Matx33f cameraMatrix;
+    settings.getCameraMatrix(cameraMatrix);
+    std::vector<int> iterCounts;
+    settings.getIterCounts(iterCounts);
+    bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix,
+                                         this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(),
+                                         iterCounts, this->settings.getMaxTranslation(),
+                                         this->settings.getMaxRotation(), settings.getSobelScale(),
+                                         OdometryType::RGB_DEPTH, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype);
+    return isCorrect;
+}
+
+bool OdometryRGBD::compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const
+{
+    CV_UNUSED(srcDepth);
+    CV_UNUSED(dstDepth);
+    CV_UNUSED(Rt);
+    CV_Error(cv::Error::StsBadFunc, "This odometry algorithm needs depth and rgb data simultaneously");
+}
+
+bool OdometryRGBD::compute(InputArray _srcDepth, InputArray _srcRGB,
+                           InputArray _dstDepth, InputArray _dstRGB, OutputArray Rt) const
+{
+    OdometryFrame srcFrame(_srcDepth, _srcRGB);
+    OdometryFrame dstFrame(_dstDepth, _dstRGB);
+
+    prepareRGBDFrame(srcFrame, dstFrame, this->normalsComputer, this->settings, this->algtype);
+    bool isCorrect = compute(srcFrame, dstFrame, Rt);
+    return isCorrect;
+}
+
+
+Odometry::Odometry()
+{
+    OdometrySettings settings;
+    this->impl = makePtr<OdometryICP>(settings, OdometryAlgoType::COMMON);
+}
+
+Odometry::Odometry(OdometryType otype)
+{
+    OdometrySettings settings;
+    switch (otype)
+    {
+    case OdometryType::DEPTH:
+        this->impl = makePtr<OdometryICP>(settings, OdometryAlgoType::FAST);
+        break;
+    case OdometryType::RGB:
+        this->impl = makePtr<OdometryRGB>(settings, OdometryAlgoType::COMMON);
+        break;
+    case OdometryType::RGB_DEPTH:
+        this->impl = makePtr<OdometryRGBD>(settings, OdometryAlgoType::COMMON);
+        break;
+    default:
+        CV_Error(Error::StsInternal,
+            "Incorrect OdometryType, you are able to use only { DEPTH = 0, RGB = 1, RGB_DEPTH = 2 }");
+        break;
+    }
+}
+
+Odometry::Odometry(OdometryType otype, const OdometrySettings& settings, OdometryAlgoType algtype)
+{
+    switch (otype)
+    {
+    case OdometryType::DEPTH:
+        this->impl = makePtr<OdometryICP>(settings, algtype);
+        break;
+    case OdometryType::RGB:
+        this->impl = makePtr<OdometryRGB>(settings, algtype);
+        break;
+    case OdometryType::RGB_DEPTH:
+        this->impl = makePtr<OdometryRGBD>(settings, algtype);
+        break;
+    default:
+        CV_Error(Error::StsInternal,
+                 "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }");
+        break;
+    }
+}
+
+Odometry::~Odometry()
+{
+}
+
+void Odometry::prepareFrame(OdometryFrame& frame) const
+{
+    this->impl->prepareFrame(frame);
+}
+
+void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const
+{
+    this->impl->prepareFrames(srcFrame, dstFrame);
+}
+
+bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const
+{
+    return this->impl->compute(srcFrame, dstFrame, Rt);
+}
+
+bool Odometry::compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const
+{
+    return this->impl->compute(srcDepth, dstDepth, Rt);
+}
+
+bool Odometry::compute(InputArray srcDepth, InputArray srcRGB,
+                       InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const
+{
+    return this->impl->compute(srcDepth, srcRGB, dstDepth, dstRGB, Rt);
+}
+
+Ptr<RgbdNormals> Odometry::getNormalsComputer() const
+{
+    return this->impl->getNormalsComputer();
+}
+
+
+void warpFrame(InputArray depth, InputArray image, InputArray mask,
+               InputArray Rt, InputArray cameraMatrix,
+               OutputArray warpedDepth, OutputArray warpedImage, OutputArray warpedMask)
+{
+    CV_Assert(cameraMatrix.size() == Size(3, 3));
+    CV_Assert(cameraMatrix.depth() == CV_32F || cameraMatrix.depth() == CV_64F);
+    Matx33d K, Kinv;
+    cameraMatrix.getMat().convertTo(K, CV_64F);
+    std::vector<bool> camPlaces { /* fx */ true, false, /* cx */ true, false, /* fy */ true, /* cy */ true,  false, false, /* 1 */ true};
+    for (int i = 0; i < 9; i++)
+    {
+        CV_Assert(camPlaces[i] == (K.val[i] > DBL_EPSILON));
+    }
+    Kinv = K.inv();
+
+    CV_Assert((Rt.cols() == 4) && (Rt.rows() == 3 || Rt.rows() == 4));
+    CV_Assert(Rt.depth() == CV_32F || Rt.depth() == CV_64F);
+    Mat rtmat;
+    Rt.getMat().convertTo(rtmat, CV_64F);
+    Affine3d rt(rtmat);
+
+    CV_Assert(!depth.empty());
+    CV_Assert(depth.channels() == 1);
+    double maxDepth = 0;
+    int depthDepth = depth.depth();
+    switch (depthDepth)
+    {
+    case CV_16U:
+        maxDepth = std::numeric_limits<unsigned short>::max();
+        break;
+    case CV_32F:
+        maxDepth = std::numeric_limits<float>::max();
+        break;
+    case CV_64F:
+        maxDepth = std::numeric_limits<double>::max();
+        break;
+    default:
+        CV_Error(Error::StsBadArg, "Unsupported depth data type");
+    }
+    Mat_<double> depthDbl;
+    depth.getMat().convertTo(depthDbl, CV_64F);
+    Size sz = depth.size();
+
+    Mat_<uchar> maskMat;
+    if (!mask.empty())
+    {
+        CV_Assert(mask.type() == CV_8UC1 || mask.type() == CV_8SC1 || mask.type() == CV_BoolC1);
+        CV_Assert(mask.size() == sz);
+        maskMat = mask.getMat();
+    }
+
+    int imageType = -1;
+    Mat imageMat;
+    if (!image.empty())
+    {
+        imageType = image.type();
+        CV_Assert(imageType == CV_8UC1 || imageType == CV_8UC3 || imageType == CV_8UC4);
+        CV_Assert(image.size() == sz);
+        CV_Assert(warpedImage.needed());
+        imageMat = image.getMat();
+    }
+
+    CV_Assert(warpedDepth.needed() || warpedImage.needed() || warpedMask.needed());
+
+    // Getting new coords for depth point
+
+    // see the explanation in the loop below
+    Matx33d krki = K * rt.rotation() * Kinv;
+    Matx32d krki_cols01 = krki.get_minor<3, 2>(0, 0);
+    Vec3d krki_col2(krki.col(2).val);
+
+    Vec3d ktmat = K * rt.translation();
+    Mat_<Vec3d> reprojBack(depth.size());
+    for (int y = 0; y < sz.height; y++)
+    {
+        const uchar* maskRow = maskMat.empty() ? nullptr : maskMat[y];
+        const double* depthRow = depthDbl[y];
+        Vec3d* reprojRow = reprojBack[y];
+        for (int x = 0; x < sz.width; x++)
+        {
+            double z = depthRow[x];
+            bool badz = cvIsNaN(z) || cvIsInf(z) || z <= 0 || z >= maxDepth || (maskRow && !maskRow[x]);
+            Vec3d v;
+            if (!badz)
+            {
+                // Reproject pixel (x, y) using known z, rotate+translate and project back
+                // getting new pixel in projective coordinates:
+                // v = K * Rt * K^-1 * ([x, y, 1] * z) = [new_x*new_z, new_y*new_z, new_z]
+                // v = K * (R * K^-1 * ([x, y, 1] * z) + t) =
+                // v = krki * [x, y, 1] * z + ktmat =
+                // v = (krki_cols01 * [x, y] + krki_col2) * z + K * t
+                v = (krki_cols01 * Vec2d(x, y) + krki_col2) * z + ktmat;
+            }
+            else
+            {
+                v = Vec3d();
+            }
+            reprojRow[x] = v;
+        }
+    }
+
+    // Draw new depth in z-buffer manner
+
+    Mat warpedImageMat;
+    if (warpedImage.needed())
+    {
+        warpedImage.create(sz, imageType);
+        warpedImage.setZero();
+        warpedImageMat = warpedImage.getMat();
+    }
+
+    const double infinity = std::numeric_limits<double>::max();
+
+    Mat zBuffer(sz, CV_32FC1, infinity);
+
+    const Rect rect = Rect(Point(), sz);
+
+    for (int y = 0; y < sz.height; y++)
+    {
+        uchar* imageRow1ch = nullptr;
+        Vec3b* imageRow3ch = nullptr;
+        Vec4b* imageRow4ch = nullptr;
+        switch (imageType)
+        {
+        case -1:
+            break;
+        case CV_8UC1:
+            imageRow1ch = imageMat.ptr<uchar>(y);
+            break;
+        case CV_8UC3:
+            imageRow3ch = imageMat.ptr<Vec3b>(y);
+            break;
+        case CV_8UC4:
+            imageRow4ch = imageMat.ptr<Vec4b>(y);
+            break;
+        default:
+            break;
+        }
+
+        const Vec3d* reprojRow = reprojBack[y];
+        for (int x = 0; x < sz.width; x++)
+        {
+            Vec3d v = reprojRow[x];
+            double z = v[2];
+
+            if (z > 0)
+            {
+                Point uv(cvFloor(v[0] / z), cvFloor(v[1] / z));
+                if (rect.contains(uv))
+                {
+                    float oldz = zBuffer.at<float>(uv);
+
+                    if (z < oldz)
+                    {
+                        zBuffer.at<float>(uv) = (float)z;
+
+                        switch (imageType)
+                        {
+                        case -1:
+                            break;
+                        case CV_8UC1:
+                            warpedImageMat.at<uchar>(uv) = imageRow1ch[x];
+                            break;
+                        case CV_8UC3:
+                            warpedImageMat.at<Vec3b>(uv) = imageRow3ch[x];
+                            break;
+                        case CV_8UC4:
+                            warpedImageMat.at<Vec4b>(uv) = imageRow4ch[x];
+                            break;
+                        default:
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    if (warpedDepth.needed() || warpedMask.needed())
+    {
+        Mat goodMask = (zBuffer < infinity);
+
+        if (warpedDepth.needed())
+        {
+            warpedDepth.create(sz, depthDepth);
+
+            double badVal;
+            switch (depthDepth)
+            {
+            case CV_16U:
+                badVal = 0;
+                break;
+            case CV_32F:
+                badVal = std::numeric_limits<float>::quiet_NaN();
+                break;
+            case CV_64F:
+                badVal = std::numeric_limits<double>::quiet_NaN();
+                break;
+            default:
+                break;
+            }
+
+            zBuffer.convertTo(warpedDepth, depthDepth);
+            warpedDepth.setTo(badVal, ~goodMask);
+        }
+
+        if (warpedMask.needed())
+        {
+            warpedMask.create(sz, CV_8UC1);
+            goodMask.copyTo(warpedMask);
+        }
+    }
+}
+
+}

modules/ptcloud/src/odometry_frame_impl.cpp

@@ -0,0 +1,100 @@
+// 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
+
+#include "precomp.hpp"
+
+#include <opencv2/core/ocl.hpp>
+
+#include "utils.hpp"
+
+namespace cv
+{
+
+OdometryFrame::OdometryFrame(InputArray depth, InputArray image, InputArray mask, InputArray normals)
+{
+    this->impl = makePtr<OdometryFrame::Impl>();
+    if (!image.empty())
+    {
+        image.copyTo(this->impl->image);
+    }
+    if (!depth.empty())
+    {
+        depth.copyTo(this->impl->depth);
+    }
+    if (!mask.empty())
+    {
+        mask.copyTo(this->impl->mask);
+    }
+    if (!normals.empty())
+    {
+        normals.copyTo(this->impl->normals);
+    }
+}
+
+void OdometryFrame::getImage(OutputArray image) const { this->impl->getImage(image); }
+void OdometryFrame::getGrayImage(OutputArray image) const { this->impl->getGrayImage(image); }
+void OdometryFrame::getDepth(OutputArray depth) const { this->impl->getDepth(depth); }
+void OdometryFrame::getProcessedDepth(OutputArray depth) const { this->impl->getProcessedDepth(depth); }
+void OdometryFrame::getMask(OutputArray mask) const { this->impl->getMask(mask); }
+void OdometryFrame::getNormals(OutputArray normals) const { this->impl->getNormals(normals); }
+
+int OdometryFrame::getPyramidLevels() const { return this->impl->getPyramidLevels(); }
+
+void OdometryFrame::getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const
+{
+    this->impl->getPyramidAt(img, pyrType, level);
+}
+
+void OdometryFrame::Impl::getImage(OutputArray _image) const
+{
+    _image.assign(this->image);
+}
+
+void OdometryFrame::Impl::getGrayImage(OutputArray _image) const
+{
+    _image.assign(this->imageGray);
+}
+
+void OdometryFrame::Impl::getDepth(OutputArray _depth) const
+{
+    _depth.assign(this->depth);
+}
+
+void OdometryFrame::Impl::getProcessedDepth(OutputArray _depth) const
+{
+    _depth.assign(this->scaledDepth);
+}
+
+void OdometryFrame::Impl::getMask(OutputArray _mask) const
+{
+    _mask.assign(this->mask);
+}
+
+void OdometryFrame::Impl::getNormals(OutputArray _normals) const
+{
+    _normals.assign(this->normals);
+}
+
+int OdometryFrame::Impl::getPyramidLevels() const
+{
+    // all pyramids should have the same size
+    for (const auto& p : this->pyramids)
+    {
+        if (!p.empty())
+            return (int)(p.size());
+    }
+    return 0;
+}
+
+
+void OdometryFrame::Impl::getPyramidAt(OutputArray _img, OdometryFramePyramidType pyrType, size_t level) const
+{
+    CV_Assert(pyrType < OdometryFramePyramidType::N_PYRAMIDS);
+    if (level < pyramids[pyrType].size())
+        _img.assign(pyramids[pyrType][level]);
+    else
+        _img.clear();
+}
+
+}

modules/ptcloud/src/odometry_functions.hpp

@@ -0,0 +1,200 @@
+// 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_ODOMETRY_FUNCTIONS_HPP
+#define OPENCV_3D_ODOMETRY_FUNCTIONS_HPP
+
+#include "utils.hpp"
+#include <opencv2/imgproc.hpp>
+
+namespace cv
+{
+enum class OdometryTransformType
+{
+    // rotation, translation, rotation+translation
+    ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4
+};
+
+static inline int getTransformDim(OdometryTransformType transformType)
+{
+    switch(transformType)
+    {
+    case OdometryTransformType::RIGID_TRANSFORMATION:
+        return 6;
+    case OdometryTransformType::ROTATION:
+    case OdometryTransformType::TRANSLATION:
+        return 3;
+    default:
+        CV_Error(Error::StsBadArg, "Incorrect transformation type");
+    }
+}
+
+
+static inline
+Vec6d calcRgbdEquationCoeffs(double dIdx, double dIdy, const Point3d& p3d, double fx, double fy)
+{
+    double invz = 1. / p3d.z,
+           v0 = dIdx * fx * invz,
+           v1 = dIdy * fy * invz,
+           v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
+    Point3d v(v0, v1, v2);
+    Point3d pxv = p3d.cross(v);
+
+    return Vec6d(pxv.x, pxv.y, pxv.z, v0, v1, v2);
+}
+
+static inline
+Vec3d calcRgbdEquationCoeffsRotation(double dIdx, double dIdy, const Point3d& p3d, double fx, double fy)
+{
+    double invz = 1. / p3d.z,
+           v0 = dIdx * fx * invz,
+           v1 = dIdy * fy * invz,
+           v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
+
+    Point3d v(v0, v1, v2);
+    Point3d pxv = p3d.cross(v);
+
+    return Vec3d(pxv);
+}
+
+static inline
+Vec3d calcRgbdEquationCoeffsTranslation(double dIdx, double dIdy, const Point3d& p3d, double fx, double fy)
+{
+    double invz = 1. / p3d.z,
+           v0 = dIdx * fx * invz,
+           v1 = dIdy * fy * invz,
+           v2 = -(v0 * p3d.x + v1 * p3d.y) * invz;
+
+    return Vec3d(v0, v1, v2);
+}
+
+static inline void rgbdCoeffsFunc(OdometryTransformType transformType,
+                                  double* C, double dIdx, double dIdy, const Point3d& p3d, double fx, double fy)
+{
+    int dim = getTransformDim(transformType);
+    Vec6d ret;
+    switch(transformType)
+    {
+    case OdometryTransformType::RIGID_TRANSFORMATION:
+    {
+        ret = calcRgbdEquationCoeffs(dIdx, dIdy, p3d, fx, fy);
+        break;
+    }
+    case OdometryTransformType::ROTATION:
+    {
+        Vec3d r = calcRgbdEquationCoeffsRotation(dIdx, dIdy, p3d, fx, fy);
+        ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
+        break;
+    }
+    case OdometryTransformType::TRANSLATION:
+    {
+        Vec3d r = calcRgbdEquationCoeffsTranslation(dIdx, dIdy, p3d, fx, fy);
+        ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
+        break;
+    }
+    default:
+        CV_Error(Error::StsBadArg, "Incorrect transformation type");
+    }
+    for (int i = 0; i < dim; i++)
+        C[i] = ret[i];
+}
+
+
+static inline
+Vec6d calcICPEquationCoeffs(const Point3d& psrc, const Vec3d& ndst)
+{
+    Point3d pxv = psrc.cross(Point3d(ndst));
+
+    return Vec6d(pxv.x, pxv.y, pxv.z, ndst[0], ndst[1], ndst[2]);
+}
+
+static inline
+Vec3d calcICPEquationCoeffsRotation(const Point3d& psrc, const Vec3d& ndst)
+{
+    Point3d pxv = psrc.cross(Point3d(ndst));
+
+    return Vec3d(pxv);
+}
+
+static inline
+Vec3d calcICPEquationCoeffsTranslation( const Point3d& /*p0*/, const Vec3d& ndst)
+{
+    return Vec3d(ndst);
+}
+
+static inline
+void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3d& p0, const Point3d& /*p1*/, const Vec3d& n1)
+{
+    int dim = getTransformDim(transformType);
+    Vec6d ret;
+    switch(transformType)
+    {
+    case OdometryTransformType::RIGID_TRANSFORMATION:
+    {
+        ret = calcICPEquationCoeffs(p0, n1);
+        break;
+    }
+    case OdometryTransformType::ROTATION:
+    {
+        Vec3d r = calcICPEquationCoeffsRotation(p0, n1);
+        ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
+        break;
+    }
+    case OdometryTransformType::TRANSLATION:
+    {
+        Vec3d r = calcICPEquationCoeffsTranslation(p0, n1);
+        ret = Vec6d(r[0], r[1], r[2], 0, 0, 0);
+        break;
+    }
+    default:
+        CV_Error(Error::StsBadArg, "Incorrect transformation type");
+    }
+    for (int i = 0; i < dim; i++)
+        C[i] = ret[i];
+}
+
+void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr<RgbdNormals>& normalsComputer, const OdometrySettings settings, OdometryAlgoType algtype);
+void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings);
+void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, Ptr<RgbdNormals>& normalsComputer, const OdometrySettings settings, OdometryAlgoType algtype);
+
+bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
+                         const OdometryFrame& srcFrame,
+                         const OdometryFrame& dstFrame,
+                         const Matx33f& cameraMatrix,
+                         float maxDepthDiff, float angleThreshold, const std::vector<int>& iterCounts,
+                         double maxTranslation, double maxRotation, double sobelScale,
+                         OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype);
+
+void computeCorresps(const Matx33f& _K, const Mat& Rt,
+                     const Mat& image0, const Mat& depth0, const Mat& validMask0,
+                     const Mat& image1, const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
+                     Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method);
+
+void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt,
+                         const Mat& dI_dx1, const Mat& dI_dy1,
+                         const Mat& corresps, const Mat& diffs, const double sigma,
+                         double fx, double fy, double sobelScaleIn,
+                         Mat& AtA, Mat& AtB, OdometryTransformType transformType);
+
+void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
+                        const Mat& cloud1, const Mat& normals1,
+                        const Mat& corresps,
+                        Mat& AtA, Mat& AtB, OdometryTransformType transformType);
+
+void calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm,
+                            cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);
+
+#ifdef HAVE_OPENCL
+bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm,
+                                cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b);
+#endif
+
+void computeProjectiveMatrix(const Mat& ksi, Mat& Rt);
+
+bool solveSystem(const Mat& AtA, const Mat& AtB, double detThreshold, Mat& x);
+
+bool testDeltaTransformation(const Mat& deltaRt, double maxTranslation, double maxRotation);
+
+}
+#endif //OPENCV_3D_ODOMETRY_FUNCTIONS_HPP

modules/ptcloud/src/odometry_settings_impl.cpp

@@ -0,0 +1,407 @@
+// 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
+
+#include "precomp.hpp"
+#include "utils.hpp"
+
+namespace cv
+{
+
+class OdometrySettings::Impl
+{
+public:
+    Impl() {};
+    virtual ~Impl() {};
+    virtual void setCameraMatrix(InputArray val) = 0;
+    virtual void getCameraMatrix(OutputArray val) const = 0;
+    virtual void setIterCounts(InputArray val) = 0;
+    virtual void getIterCounts(OutputArray val) const = 0;
+
+    virtual void  setMinDepth(float val) = 0;
+    virtual float getMinDepth() const = 0;
+    virtual void  setMaxDepth(float val) = 0;
+    virtual float getMaxDepth() const = 0;
+    virtual void  setMaxDepthDiff(float val) = 0;
+    virtual float getMaxDepthDiff() const = 0;
+    virtual void  setMaxPointsPart(float val) = 0;
+    virtual float getMaxPointsPart() const = 0;
+
+    virtual void setSobelSize(int val) = 0;
+    virtual int  getSobelSize() const = 0;
+    virtual void   setSobelScale(double val) = 0;
+    virtual double getSobelScale() const = 0;
+
+    virtual void setNormalWinSize(int val) = 0;
+    virtual int  getNormalWinSize() const = 0;
+    virtual void setNormalDiffThreshold(float val) = 0;
+    virtual float getNormalDiffThreshold() const = 0;
+    virtual void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) = 0;
+    virtual RgbdNormals::RgbdNormalsMethod getNormalMethod() const = 0;
+
+    virtual void  setAngleThreshold(float val) = 0;
+    virtual float getAngleThreshold() const = 0;
+    virtual void  setMaxTranslation(float val) = 0;
+    virtual float getMaxTranslation() const = 0;
+    virtual void  setMaxRotation(float val) = 0;
+    virtual float getMaxRotation() const = 0;
+
+    virtual void  setMinGradientMagnitude(float val) = 0;
+    virtual float getMinGradientMagnitude() const = 0;
+    virtual void setMinGradientMagnitudes(InputArray val) = 0;
+    virtual void getMinGradientMagnitudes(OutputArray val) const = 0;
+};
+
+class OdometrySettingsImplCommon : public OdometrySettings::Impl
+{
+public:
+    OdometrySettingsImplCommon();
+    ~OdometrySettingsImplCommon() {};
+    virtual void setCameraMatrix(InputArray val) override;
+    virtual void getCameraMatrix(OutputArray val) const override;
+    virtual void setIterCounts(InputArray val) override;
+    virtual void getIterCounts(OutputArray val) const override;
+
+    virtual void  setMinDepth(float val) override;
+    virtual float getMinDepth() const override;
+    virtual void  setMaxDepth(float val) override;
+    virtual float getMaxDepth() const override;
+    virtual void  setMaxDepthDiff(float val) override;
+    virtual float getMaxDepthDiff() const override;
+    virtual void  setMaxPointsPart(float val) override;
+    virtual float getMaxPointsPart() const override;
+
+    virtual void setSobelSize(int val) override;
+    virtual int  getSobelSize() const override;
+    virtual void   setSobelScale(double val) override;
+    virtual double getSobelScale() const override;
+
+    virtual void setNormalWinSize(int val) override;
+    virtual int  getNormalWinSize() const override;
+    virtual void setNormalDiffThreshold(float val) override;
+    virtual float getNormalDiffThreshold() const override;
+    virtual void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) override;
+    virtual RgbdNormals::RgbdNormalsMethod getNormalMethod() const override;
+
+    virtual void  setAngleThreshold(float val) override;
+    virtual float getAngleThreshold() const override;
+    virtual void  setMaxTranslation(float val) override;
+    virtual float getMaxTranslation() const override;
+    virtual void  setMaxRotation(float val) override;
+    virtual float getMaxRotation() const override;
+
+    virtual void  setMinGradientMagnitude(float val) override;
+    virtual float getMinGradientMagnitude() const override;
+    virtual void setMinGradientMagnitudes(InputArray val) override;
+    virtual void getMinGradientMagnitudes(OutputArray val) const override;
+
+private:
+    Matx33f cameraMatrix;
+    std::vector<int> iterCounts;
+
+    float minDepth;
+    float maxDepth;
+    float maxDepthDiff;
+    float maxPointsPart;
+
+    int sobelSize;
+    double sobelScale;
+
+    int normalWinSize;
+    float normalDiffThreshold;
+    RgbdNormals::RgbdNormalsMethod normalMethod;
+
+    float angleThreshold;
+    float maxTranslation;
+    float maxRotation;
+
+    float minGradientMagnitude;
+    std::vector<float> minGradientMagnitudes;
+
+public:
+    class DefaultSets {
+    public:
+        static const int width  = 640;
+        static const int height = 480;
+        static constexpr float fx = 525.f;
+        static constexpr float fy = 525.f;
+        static constexpr float cx = float(width) / 2.f - 0.5f;
+        static constexpr float cy = float(height) / 2.f - 0.5f;
+        const cv::Matx33f defaultCameraMatrix = { fx, 0, cx, 0, fy, cy, 0, 0, 1 };
+        const std::vector<int> defaultIterCounts = { 7, 7, 7, 10 };
+
+        static constexpr float defaultMinDepth = 0.f;
+        static constexpr float defaultMaxDepth = 4.f;
+        static constexpr float defaultMaxDepthDiff = 0.07f;
+        static constexpr float defaultMaxPointsPart = 0.07f;
+
+        static const int defaultSobelSize = 3;
+        static constexpr double defaultSobelScale = 1. / 8.;
+
+        static const int defaultNormalWinSize = 5;
+        static const RgbdNormals::RgbdNormalsMethod defaultNormalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
+        static constexpr float defaultNormalDiffThreshold = 50.f;
+
+        static constexpr float defaultAngleThreshold = (float)(30. * CV_PI / 180.);
+        static constexpr float defaultMaxTranslation = 0.15f;
+        static constexpr float defaultMaxRotation = 15.f;
+
+        static constexpr float defaultMinGradientMagnitude = 10.f;
+        const std::vector<float> defaultMinGradientMagnitudes = std::vector<float>(defaultIterCounts.size(), 10.f /*defaultMinGradientMagnitude*/);
+    };
+};
+
+
+OdometrySettings::OdometrySettings()
+{
+    this->impl = makePtr<OdometrySettingsImplCommon>();
+}
+
+OdometrySettings::OdometrySettings(const OdometrySettings& s)
+{
+    this->impl = makePtr<OdometrySettingsImplCommon>(*s.impl.dynamicCast<OdometrySettingsImplCommon>());
+}
+
+OdometrySettings& OdometrySettings::operator=(const OdometrySettings& s)
+{
+    this->impl = makePtr<OdometrySettingsImplCommon>(*s.impl.dynamicCast<OdometrySettingsImplCommon>());
+    return *this;
+}
+
+void OdometrySettings::setCameraMatrix(InputArray val) { this->impl->setCameraMatrix(val); }
+void OdometrySettings::getCameraMatrix(OutputArray val) const { this->impl->getCameraMatrix(val); }
+void OdometrySettings::setIterCounts(InputArray val) { this->impl->setIterCounts(val); }
+void OdometrySettings::getIterCounts(OutputArray val) const { this->impl->getIterCounts(val); }
+
+void  OdometrySettings::setMinDepth(float val) { this->impl->setMinDepth(val); }
+float OdometrySettings::getMinDepth() const { return this->impl->getMinDepth(); }
+void  OdometrySettings::setMaxDepth(float val) { this->impl->setMaxDepth(val); }
+float OdometrySettings::getMaxDepth() const { return this->impl->getMaxDepth(); }
+void  OdometrySettings::setMaxDepthDiff(float val) { this->impl->setMaxDepthDiff(val); }
+float OdometrySettings::getMaxDepthDiff() const { return this->impl->getMaxDepthDiff(); }
+void  OdometrySettings::setMaxPointsPart(float val) { this->impl->setMaxPointsPart(val); }
+float OdometrySettings::getMaxPointsPart() const { return this->impl->getMaxPointsPart(); }
+
+void OdometrySettings::setSobelSize(int val) { this->impl->setSobelSize(val); }
+int  OdometrySettings::getSobelSize() const { return this->impl->getSobelSize(); }
+void   OdometrySettings::setSobelScale(double val) { this->impl->setSobelScale(val); }
+double OdometrySettings::getSobelScale() const { return this->impl->getSobelScale(); }
+
+void  OdometrySettings::setNormalWinSize(int val) { this->impl->setNormalWinSize(val); }
+int   OdometrySettings::getNormalWinSize() const { return this->impl->getNormalWinSize(); }
+void  OdometrySettings::setNormalDiffThreshold(float val) { this->impl->setNormalDiffThreshold(val); }
+float OdometrySettings::getNormalDiffThreshold() const { return this->impl->getNormalDiffThreshold(); }
+void  OdometrySettings::setNormalMethod(RgbdNormals::RgbdNormalsMethod nm) { this->impl->setNormalMethod(nm); }
+RgbdNormals::RgbdNormalsMethod OdometrySettings::getNormalMethod() const { return this->impl->getNormalMethod(); }
+
+void  OdometrySettings::setAngleThreshold(float val) { this->impl->setAngleThreshold(val); }
+float OdometrySettings::getAngleThreshold() const { return this->impl->getAngleThreshold(); }
+void  OdometrySettings::setMaxTranslation(float val) { this->impl->setMaxTranslation(val); }
+float OdometrySettings::getMaxTranslation() const { return this->impl->getMaxTranslation(); }
+void  OdometrySettings::setMaxRotation(float val) { this->impl->setMaxRotation(val); }
+float OdometrySettings::getMaxRotation() const { return this->impl->getMaxRotation(); }
+
+void  OdometrySettings::setMinGradientMagnitude(float val) { this->impl->setMinGradientMagnitude(val); }
+float OdometrySettings::getMinGradientMagnitude() const { return this->impl->getMinGradientMagnitude(); }
+void OdometrySettings::setMinGradientMagnitudes(InputArray val) { this->impl->setMinGradientMagnitudes(val); }
+void OdometrySettings::getMinGradientMagnitudes(OutputArray val) const { this->impl->getMinGradientMagnitudes(val); }
+
+
+OdometrySettingsImplCommon::OdometrySettingsImplCommon()
+{
+    DefaultSets ds;
+    this->cameraMatrix = ds.defaultCameraMatrix;
+    this->iterCounts = ds.defaultIterCounts;
+
+    this->minDepth = ds.defaultMinDepth;
+    this->maxDepth = ds.defaultMaxDepth;
+    this->maxDepthDiff = ds.defaultMaxDepthDiff;
+    this->maxPointsPart = ds.defaultMaxPointsPart;
+
+    this->sobelSize = ds.defaultSobelSize;
+    this->sobelScale = ds.defaultSobelScale;
+
+    this->normalWinSize = ds.defaultNormalWinSize;
+    this->normalDiffThreshold = ds.defaultNormalDiffThreshold;
+    this->normalMethod = ds.defaultNormalMethod;
+
+    this->angleThreshold = ds.defaultAngleThreshold;
+    this->maxTranslation = ds.defaultMaxTranslation;
+    this->maxRotation = ds.defaultMaxRotation;
+
+    this->minGradientMagnitude = ds.defaultMinGradientMagnitude;
+    this->minGradientMagnitudes = ds.defaultMinGradientMagnitudes;
+}
+
+void OdometrySettingsImplCommon::setCameraMatrix(InputArray val)
+{
+    if (!val.empty())
+    {
+        CV_Assert(val.rows() == 3 && val.cols() == 3 && val.channels() == 1);
+        CV_Assert(val.type() == CV_32F);
+        val.copyTo(cameraMatrix);
+    }
+    else
+    {
+        DefaultSets ds;
+        this->cameraMatrix = ds.defaultCameraMatrix;
+    }
+}
+
+void OdometrySettingsImplCommon::getCameraMatrix(OutputArray val) const
+{
+    Mat(this->cameraMatrix).copyTo(val);
+}
+
+void OdometrySettingsImplCommon::setIterCounts(InputArray val)
+{
+    if (!val.empty())
+    {
+        size_t nLevels = val.size(-1).width;
+        std::vector<Mat> pyramids;
+        val.getMatVector(pyramids);
+        this->iterCounts.clear();
+        for (size_t i = 0; i < nLevels; i++)
+            this->iterCounts.push_back(pyramids[i].at<int>(0));
+    }
+    else
+    {
+        DefaultSets ds;
+        this->iterCounts = ds.defaultIterCounts;
+    }
+}
+
+void OdometrySettingsImplCommon::getIterCounts(OutputArray val) const
+{
+    Mat(this->iterCounts).copyTo(val);
+}
+
+void OdometrySettingsImplCommon::setMinDepth(float val)
+{
+    this->minDepth = val;
+}
+float OdometrySettingsImplCommon::getMinDepth() const
+{
+    return this->minDepth;
+}
+void  OdometrySettingsImplCommon::setMaxDepth(float val)
+{
+    this->maxDepth = val;
+}
+float OdometrySettingsImplCommon::getMaxDepth() const
+{
+    return this->maxDepth;
+}
+void  OdometrySettingsImplCommon::setMaxDepthDiff(float val)
+{
+    this->maxDepthDiff = val;
+}
+float OdometrySettingsImplCommon::getMaxDepthDiff() const
+{
+    return this->maxDepthDiff;
+}
+void  OdometrySettingsImplCommon::setMaxPointsPart(float val)
+{
+    this->maxPointsPart = val;
+}
+float OdometrySettingsImplCommon::getMaxPointsPart() const
+{
+    return this->maxPointsPart;
+}
+
+void OdometrySettingsImplCommon::setSobelSize(int val)
+{
+    this->sobelSize = val;
+}
+int  OdometrySettingsImplCommon::getSobelSize() const
+{
+    return this->sobelSize;
+}
+void   OdometrySettingsImplCommon::setSobelScale(double val)
+{
+    this->sobelScale = val;
+}
+double OdometrySettingsImplCommon::getSobelScale() const
+{
+    return this->sobelScale;
+}
+void OdometrySettingsImplCommon::setNormalWinSize(int val)
+{
+    this->normalWinSize = val;
+}
+int  OdometrySettingsImplCommon::getNormalWinSize() const
+{
+    return this->normalWinSize;
+}
+void OdometrySettingsImplCommon::setNormalDiffThreshold(float val)
+{
+    this->normalDiffThreshold = val;
+}
+float OdometrySettingsImplCommon::getNormalDiffThreshold() const
+{
+    return this->normalDiffThreshold;
+}
+void OdometrySettingsImplCommon::setNormalMethod(RgbdNormals::RgbdNormalsMethod nm)
+{
+    this->normalMethod = nm;
+}
+RgbdNormals::RgbdNormalsMethod OdometrySettingsImplCommon::getNormalMethod() const
+{
+    return this->normalMethod;
+}
+void  OdometrySettingsImplCommon::setAngleThreshold(float val)
+{
+    this->angleThreshold = val;
+}
+float OdometrySettingsImplCommon::getAngleThreshold() const
+{
+    return this->angleThreshold;
+}
+void  OdometrySettingsImplCommon::setMaxTranslation(float val)
+{
+    this->maxTranslation = val;
+}
+float OdometrySettingsImplCommon::getMaxTranslation() const
+{
+    return this->maxTranslation;
+}
+void  OdometrySettingsImplCommon::setMaxRotation(float val)
+{
+    this->maxRotation = val;
+}
+float OdometrySettingsImplCommon::getMaxRotation() const
+{
+    return this->maxRotation;
+}
+
+void  OdometrySettingsImplCommon::setMinGradientMagnitude(float val)
+{
+    this->minGradientMagnitude = val;
+}
+float OdometrySettingsImplCommon::getMinGradientMagnitude() const
+{
+    return this->minGradientMagnitude;
+}
+void OdometrySettingsImplCommon::setMinGradientMagnitudes(InputArray val)
+{
+    if (!val.empty())
+    {
+        size_t nLevels = val.size(-1).width;
+        std::vector<Mat> pyramids;
+        val.getMatVector(pyramids);
+        this->minGradientMagnitudes.clear();
+        for (size_t i = 0; i < nLevels; i++)
+            this->minGradientMagnitudes.push_back(pyramids[i].at<float>(0));
+    }
+    else
+    {
+        DefaultSets ds;
+        this->minGradientMagnitudes = ds.defaultMinGradientMagnitudes;
+    }
+}
+void OdometrySettingsImplCommon::getMinGradientMagnitudes(OutputArray val) const
+{
+    Mat(this->minGradientMagnitudes).copyTo(val);
+}
+
+}

modules/ptcloud/src/opencl/hash_tsdf.cl

@@ -0,0 +1,653 @@
+// 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
+
+#define USE_INTERPOLATION_IN_GETNORMAL 1
+#define HASH_DIVISOR 32768
+
+typedef char int8_t;
+typedef uint int32_t;
+
+typedef int8_t TsdfType;
+typedef uchar WeightType;
+
+struct TsdfVoxel
+{
+    TsdfType tsdf;
+    WeightType weight;
+};
+
+
+static inline TsdfType floatToTsdf(float num)
+{
+    int8_t res = (int8_t) ( (num * (-128)) );
+    res = res ? res : (num < 0 ? 1 : -1);
+    return res;
+}
+
+static inline float tsdfToFloat(TsdfType num)
+{
+    return ( (float) num ) / (-128);
+}
+
+static uint calc_hash(int3 x)
+{
+    unsigned int seed = 0;
+    unsigned int GOLDEN_RATIO = 0x9e3779b9;
+    seed ^= x.s0 + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+    seed ^= x.s1 + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+    seed ^= x.s2 + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+    return seed;
+}
+
+
+//TODO: make hashDivisor a power of 2
+//TODO: put it to this .cl file as a constant
+static int custom_find(int3 idx, const int hashDivisor, __global const int* hashes,
+                    __global const int4* data)
+{
+    int hash = calc_hash(idx) % hashDivisor;
+    int place = hashes[hash];
+    // search a place
+    while (place >= 0)
+    {
+        if (all(data[place].s012 == idx))
+            break;
+        else
+            place = data[place].s3;
+    }
+
+    return place;
+}
+
+
+
+static void integrateVolumeUnit(
+                        int x, int y,
+                        __global const char * depthptr,
+                        int depth_step, int depth_offset,
+                        int depth_rows, int depth_cols,
+                        __global struct TsdfVoxel * volumeptr,
+                        const __global char * pixNormsPtr,
+                        int pixNormsStep, int pixNormsOffset,
+                        int pixNormsRows, int pixNormsCols,
+                        const float16 vol2camMatrix,
+                        const float voxelSize,
+                        const int4 volResolution4,
+                        const int4 volStrides4,
+                        const float2 fxy,
+                        const float2 cxy,
+                        const float dfac,
+                        const float truncDist,
+                        const int maxWeight
+                        )
+{
+    const int3 volResolution = volResolution4.xyz;
+
+    if(x >= volResolution.x || y >= volResolution.y)
+        return;
+
+    // coord-independent constants
+    const int3 volStrides = volStrides4.xyz;
+    const float2 limits = (float2)(depth_cols-1, depth_rows-1);
+
+    const float4 vol2cam0 = vol2camMatrix.s0123;
+    const float4 vol2cam1 = vol2camMatrix.s4567;
+    const float4 vol2cam2 = vol2camMatrix.s89ab;
+
+    const float truncDistInv = 1.f/truncDist;
+
+    // optimization of camSpace transformation (vector addition instead of matmul at each z)
+    float4 inPt = (float4)(x*voxelSize, y*voxelSize, 0, 1);
+    float3 basePt = (float3)(dot(vol2cam0, inPt),
+                             dot(vol2cam1, inPt),
+                             dot(vol2cam2, inPt));
+
+    float3 camSpacePt = basePt;
+
+    // zStep == vol2cam*(float3(x, y, 1)*voxelSize) - basePt;
+    float3 zStep = ((float3)(vol2cam0.z, vol2cam1.z, vol2cam2.z))*voxelSize;
+
+    int volYidx = x*volStrides.x + y*volStrides.y;
+
+    int startZ, endZ;
+    if(fabs(zStep.z) > 1e-5f)
+    {
+        int baseZ = convert_int(-basePt.z / zStep.z);
+        if(zStep.z > 0)
+        {
+            startZ = baseZ;
+            endZ = volResolution.z;
+        }
+        else
+        {
+            startZ = 0;
+            endZ = baseZ;
+        }
+    }
+    else
+    {
+        if(basePt.z > 0)
+        {
+            startZ = 0; endZ = volResolution.z;
+        }
+        else
+        {
+            // z loop shouldn't be performed
+            startZ = endZ = 0;
+        }
+    }
+
+    startZ = max(0, startZ);
+    endZ = min(volResolution.z, endZ);
+
+    for(int z = startZ; z < endZ; z++)
+    {
+        // optimization of the following:
+        //float3 camSpacePt = vol2cam * ((float3)(x, y, z)*voxelSize);
+        camSpacePt += zStep;
+
+        if(camSpacePt.z <= 0)
+            continue;
+
+        float3 camPixVec = camSpacePt / camSpacePt.z;
+        float2 projected = mad(camPixVec.xy, fxy, cxy); // mad(a,b,c) = a * b + c
+
+        float v;
+        // bilinearly interpolate depth at projected
+        if(all(projected >= 0) && all(projected < limits))
+        {
+            float2 ip = floor(projected);
+            int xi = ip.x, yi = ip.y;
+
+            __global const float* row0 = (__global const float*)(depthptr + depth_offset +
+                                                                 (yi+0)*depth_step);
+            __global const float* row1 = (__global const float*)(depthptr + depth_offset +
+                                                                 (yi+1)*depth_step);
+
+            float v00 = row0[xi+0];
+            float v01 = row0[xi+1];
+            float v10 = row1[xi+0];
+            float v11 = row1[xi+1];
+            float4 vv = (float4)(v00, v01, v10, v11);
+
+            // assume correct depth is positive
+            if(all(vv > 0))
+            {
+                float2 t = projected - ip;
+                float2 vf = mix(vv.xz, vv.yw, t.x);
+                v = mix(vf.s0, vf.s1, t.y);
+            }
+            else
+                continue;
+        }
+        else
+            continue;
+
+        if(v == 0)
+            continue;
+
+        int2 projInt = convert_int2(projected);
+        float pixNorm = *(__global const float*)(pixNormsPtr + pixNormsOffset + projInt.y*pixNormsStep + projInt.x*sizeof(float));
+        //float pixNorm = length(camPixVec);
+
+        // difference between distances of point and of surface to camera
+        float sdf = pixNorm*(v*dfac - camSpacePt.z);
+        // possible alternative is:
+        // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0);
+        if(sdf >= -truncDist)
+        {
+            float tsdf = fmin(1.0f, sdf * truncDistInv);
+            int volIdx = volYidx + z*volStrides.z;
+
+            struct TsdfVoxel voxel = volumeptr[volIdx];
+            float value  = tsdfToFloat(voxel.tsdf);
+            int weight = voxel.weight;
+            // update TSDF
+            value = (value*weight + tsdf) / (weight + 1);
+            weight = min(weight + 1, maxWeight);
+
+            voxel.tsdf = floatToTsdf(value);
+            voxel.weight = weight;
+            volumeptr[volIdx] = voxel;
+        }
+    }
+
+}
+
+
+__kernel void integrateAllVolumeUnits(
+                        // depth
+                        __global const char * depthptr,
+                        int depth_step, int depth_offset,
+                        int depth_rows, int depth_cols,
+                        // hashMap
+                        __global const int* hashes,
+                        __global const int4* data,
+                        // volUnitsData
+                        __global struct TsdfVoxel * allVolumePtr,
+                        int table_step, int table_offset,
+                        int table_rows, int table_cols,
+                        // pixNorms
+                        const __global char * pixNormsPtr,
+                        int pixNormsStep, int pixNormsOffset,
+                        int pixNormsRows, int pixNormsCols,
+                        // isActiveFlags
+                        __global const uchar* isActiveFlagsPtr,
+                        int isActiveFlagsStep, int isActiveFlagsOffset,
+                        int isActiveFlagsRows, int isActiveFlagsCols,
+                        // cam matrix:
+                        const float16 vol2cam,
+                        // scalars:
+                        const float voxelSize,
+                        const int volUnitResolution,
+                        const int4 volStrides4,
+                        const float2 fxy,
+                        const float2 cxy,
+                        const float dfac,
+                        const float truncDist,
+                        const int maxWeight
+                        )
+{
+    const int hash_divisor = HASH_DIVISOR;
+    int i = get_global_id(0);
+    int j = get_global_id(1);
+    int row = get_global_id(2);
+    int3 idx = data[row].xyz;
+
+    const int4 volResolution4 = (int4)(volUnitResolution,
+                                       volUnitResolution,
+                                       volUnitResolution,
+                                       volUnitResolution);
+
+    int isActive = *(__global const uchar*)(isActiveFlagsPtr + isActiveFlagsOffset + row);
+
+    if (isActive)
+    {
+        int volCubed = volUnitResolution * volUnitResolution * volUnitResolution;
+        __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*)
+                                                (allVolumePtr + table_offset + row * volCubed);
+
+        // volUnit2cam = world2cam * volUnit2world =
+        // camPoseInv * volUnitPose = camPoseInv * (volPose + volPoseRot*(idx * volUnitSize)) =
+        // camPoseInv * (volPose + volPoseRot*(idx * volUnitResolution * voxelSize)) =
+        // camPoseInv * (volPose + volPoseRot*mulIdx) = camPoseInv * volPose + camPoseInv * volPoseRot * mulIdx =
+        // vol2cam + camPoseInv * volPoseRot * mulIdx
+        float3 mulIdx = convert_float3(idx * volUnitResolution) * voxelSize;
+        float16 volUnit2cam = vol2cam;
+
+        volUnit2cam.s37b += (float3)(dot(mulIdx, vol2cam.s012),
+                                     dot(mulIdx, vol2cam.s456),
+                                     dot(mulIdx, vol2cam.s89a));
+
+        integrateVolumeUnit(
+            i, j,
+            depthptr,
+            depth_step, depth_offset,
+            depth_rows, depth_cols,
+            volumeptr,
+            pixNormsPtr,
+            pixNormsStep, pixNormsOffset,
+            pixNormsRows, pixNormsCols,
+            volUnit2cam,
+            voxelSize,
+            volResolution4,
+            volStrides4,
+            fxy,
+            cxy,
+            dfac,
+            truncDist,
+            maxWeight
+            );
+    }
+}
+
+
+static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitDegree,
+                           int3 volStrides, __global const struct TsdfVoxel * allVolumePtr, int table_offset)
+
+{
+    //! Out of bounds
+    if (any(volumeIdx >= (1 << volumeUnitDegree)) ||
+        any(volumeIdx < 0))
+    {
+        struct TsdfVoxel dummy;
+        dummy.tsdf = floatToTsdf(1.0f);
+        dummy.weight = 0;
+        return dummy;
+    }
+
+    int volCubed = 1 << (volumeUnitDegree*3);
+    __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*)
+                                          (allVolumePtr + table_offset + row * volCubed);
+    int3 ismul = volumeIdx * volStrides;
+    int coordBase = ismul.x + ismul.y + ismul.z;
+    return volData[coordBase];
+}
+
+
+static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row,
+                                     int volumeUnitDegree, int3 volStrides,
+                                     __global const struct TsdfVoxel * allVolumePtr, int table_offset)
+
+{
+    //! Out of bounds
+    if (row < 0)
+    {
+        struct TsdfVoxel dummy;
+        dummy.tsdf = floatToTsdf(1.0f);
+        dummy.weight = 0;
+        return dummy;
+    }
+
+    int3 volUnitLocalIdx = volumeIdx - (volumeUnitIdx << volumeUnitDegree);
+    int volCubed = 1 << (volumeUnitDegree*3);
+    __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*)
+                                          (allVolumePtr + table_offset + row * volCubed);
+    int3 ismul = volUnitLocalIdx * volStrides;
+    int coordBase = ismul.x + ismul.y + ismul.z;
+    return volData[coordBase];
+}
+
+inline float interpolate(float3 t, float8 vz)
+{
+    float4 vy = mix(vz.s0246, vz.s1357, t.z);
+    float2 vx = mix(vy.s02, vy.s13, t.y);
+    return mix(vx.s0, vx.s1, t.x);
+}
+
+inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allVolumePtr,
+                             int volumeUnitDegree,
+                             const int hash_divisor,
+                             __global const int* hashes,
+                             __global const int4* data,
+
+                             int3 volStrides, int table_offset)
+{
+    float3 normal = (float3) (0.0f, 0.0f, 0.0f);
+    float3 fip = floor(ptVox);
+    int3 iptVox = convert_int3(fip);
+
+    // A small hash table to reduce a number of findRow() calls
+    // -2 and lower means not queried yet
+    // -1 means not found
+    // 0+ means found
+    int iterMap[8];
+    for (int i = 0; i < 8; i++)
+    {
+        iterMap[i] = -2;
+    }
+
+#if !USE_INTERPOLATION_IN_GETNORMAL
+    int4 offsets[] = { (int4)( 1,  0,  0, 0), (int4)(-1,  0,  0, 0), (int4)( 0,  1,  0, 0), // 0-3
+                       (int4)( 0, -1,  0, 0), (int4)( 0,  0,  1, 0), (int4)( 0,  0, -1, 0)  // 4-7
+    };
+
+    const int nVals = 6;
+    float vals[6];
+#else
+    int4 offsets[]={(int4)( 0,  0,  0, 0), (int4)( 0,  0,  1, 0), (int4)( 0,  1,  0, 0), (int4)( 0,  1,  1, 0), //  0-3
+                    (int4)( 1,  0,  0, 0), (int4)( 1,  0,  1, 0), (int4)( 1,  1,  0, 0), (int4)( 1,  1,  1, 0), //  4-7
+                    (int4)(-1,  0,  0, 0), (int4)(-1,  0,  1, 0), (int4)(-1,  1,  0, 0), (int4)(-1,  1,  1, 0), //  8-11
+                    (int4)( 2,  0,  0, 0), (int4)( 2,  0,  1, 0), (int4)( 2,  1,  0, 0), (int4)( 2,  1,  1, 0), // 12-15
+                    (int4)( 0, -1,  0, 0), (int4)( 0, -1,  1, 0), (int4)( 1, -1,  0, 0), (int4)( 1, -1,  1, 0), // 16-19
+                    (int4)( 0,  2,  0, 0), (int4)( 0,  2,  1, 0), (int4)( 1,  2,  0, 0), (int4)( 1,  2,  1, 0), // 20-23
+                    (int4)( 0,  0, -1, 0), (int4)( 0,  1, -1, 0), (int4)( 1,  0, -1, 0), (int4)( 1,  1, -1, 0), // 24-27
+                    (int4)( 0,  0,  2, 0), (int4)( 0,  1,  2, 0), (int4)( 1,  0,  2, 0), (int4)( 1,  1,  2, 0), // 28-31
+    };
+    const int nVals = 32;
+    float vals[32];
+#endif
+
+    for (int i = 0; i < nVals; i++)
+    {
+        int3 pt = iptVox + offsets[i].s012;
+
+        // VoxelToVolumeUnitIdx()
+        int3 volumeUnitIdx = pt >> volumeUnitDegree;
+
+        int3 vand = (volumeUnitIdx & 1);
+        int dictIdx = vand.s0 + vand.s1 * 2 + vand.s2 * 4;
+
+        int it = iterMap[dictIdx];
+        if (it < -1)
+        {
+            it = custom_find(volumeUnitIdx, hash_divisor, hashes, data);
+            iterMap[dictIdx] = it;
+        }
+
+        struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnitDegree, volStrides, allVolumePtr, table_offset);
+        vals[i] = tsdfToFloat( tmp.tsdf );
+    }
+
+#if !USE_INTERPOLATION_IN_GETNORMAL
+    float3 pv, nv;
+
+    pv = (float3)(vals[0*2  ], vals[1*2  ], vals[2*2  ]);
+    nv = (float3)(vals[0*2+1], vals[1*2+1], vals[2*2+1]);
+    normal = pv - nv;
+#else
+
+    float cxv[8], cyv[8], czv[8];
+
+    // How these numbers were obtained:
+    // 1. Take the basic interpolation sequence:
+    // 000, 001, 010, 011, 100, 101, 110, 111
+    // where each digit corresponds to shift by x, y, z axis respectively.
+    // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis
+    // 3. Search corresponding values in offsets
+    const int idxxn[8] = {  8,  9, 10, 11,  0,  1,  2,  3 };
+    const int idxxp[8] = {  4,  5,  6,  7, 12, 13, 14, 15 };
+    const int idxyn[8] = { 16, 17,  0,  1, 18, 19,  4,  5 };
+    const int idxyp[8] = {  2,  3, 20, 21,  6,  7, 22, 23 };
+    const int idxzn[8] = { 24,  0, 25,  2, 26,  4, 27,  6 };
+    const int idxzp[8] = {  1, 28,  3, 29,  5, 30,  7, 31 };
+
+    float vcxp[8], vcxn[8];
+    float vcyp[8], vcyn[8];
+    float vczp[8], vczn[8];
+
+    for (int i = 0; i < 8; i++)
+    {
+        vcxp[i] = vals[idxxp[i]]; vcxn[i] = vals[idxxn[i]];
+        vcyp[i] = vals[idxyp[i]]; vcyn[i] = vals[idxyn[i]];
+        vczp[i] = vals[idxzp[i]]; vczn[i] = vals[idxzn[i]];
+    }
+
+    float8 cxp = vload8(0, vcxp), cxn = vload8(0, vcxn);
+    float8 cyp = vload8(0, vcyp), cyn = vload8(0, vcyn);
+    float8 czp = vload8(0, vczp), czn = vload8(0, vczn);
+    float8 cx = cxp - cxn;
+    float8 cy = cyp - cyn;
+    float8 cz = czp - czn;
+
+    float3 tv = ptVox - fip;
+    normal.x = interpolate(tv, cx);
+    normal.y = interpolate(tv, cy);
+    normal.z = interpolate(tv, cz);
+#endif
+
+    float norm = sqrt(dot(normal, normal));
+    return norm < 0.0001f ? nan((uint)0) : normal / norm;
+}
+
+typedef float4 ptype;
+
+__kernel void raycast(
+                    __global const int* hashes,
+                    __global const int4* data,
+                    __global char * pointsptr,
+                      int points_step, int points_offset,
+                    __global char * normalsptr,
+                      int normals_step, int normals_offset,
+                    const int2 frameSize,
+                    __global const struct TsdfVoxel * allVolumePtr,
+                        int table_step, int table_offset,
+                        int table_rows, int table_cols,
+                    float16 cam2volRotGPU,
+                    float16 vol2camRotGPU,
+                    float truncateThreshold,
+                    const float2 fixy, const float2 cxy,
+                    const float4 boxDown4, const float4 boxUp4,
+                    const float tstep,
+                    const float voxelSize,
+                    const float voxelSizeInv,
+                    float volumeUnitSize,
+                    float truncDist,
+                    int volumeUnitDegree,
+                    int4 volStrides4
+                    )
+{
+    const int hash_divisor = HASH_DIVISOR;
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= frameSize.x || y >= frameSize.y)
+        return;
+
+    float3 point  = nan((uint)0);
+    float3 normal = nan((uint)0);
+
+    const float3 camRot0  = cam2volRotGPU.s012;
+    const float3 camRot1  = cam2volRotGPU.s456;
+    const float3 camRot2  = cam2volRotGPU.s89a;
+    const float3 camTrans = cam2volRotGPU.s37b;
+
+    const float3 volRot0  = vol2camRotGPU.s012;
+    const float3 volRot1  = vol2camRotGPU.s456;
+    const float3 volRot2  = vol2camRotGPU.s89a;
+    const float3 volTrans = vol2camRotGPU.s37b;
+
+    float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f);
+    planed = (float3)(dot(planed, camRot0),
+                      dot(planed, camRot1),
+                      dot(planed, camRot2));
+
+    float3 orig = (float3) (camTrans.s0, camTrans.s1, camTrans.s2);
+    float3 dir = fast_normalize(planed);
+    float3 origScaled = orig * voxelSizeInv;
+    float3 dirScaled = dir * voxelSizeInv;
+
+    float tmin = 0;
+    float tmax = truncateThreshold;
+    float tcurr = tmin;
+    float tprev = tcurr;
+    float prevTsdf = truncDist;
+
+    int3 volStrides = volStrides4.xyz;
+
+    while (tcurr < tmax)
+    {
+        float3 currRayPosVox = origScaled + tcurr * dirScaled;
+
+        // VolumeToVolumeUnitIdx()
+        int3 currVoxel = convert_int3(floor(currRayPosVox));
+        int3 currVolumeUnitIdx = currVoxel >> volumeUnitDegree;
+
+        int row = custom_find(currVolumeUnitIdx, hash_divisor, hashes, data);
+
+        float currTsdf = prevTsdf;
+        int currWeight = 0;
+        float stepSize = 0.5 * volumeUnitSize;
+        int3 volUnitLocalIdx;
+
+        if (row >= 0)
+        {
+            volUnitLocalIdx = currVoxel - (currVolumeUnitIdx << volumeUnitDegree);
+            struct TsdfVoxel currVoxel = at(volUnitLocalIdx, row, volumeUnitDegree, volStrides, allVolumePtr, table_offset);
+
+            currTsdf = tsdfToFloat(currVoxel.tsdf);
+            currWeight = currVoxel.weight;
+            stepSize = tstep;
+        }
+
+        if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0)
+        {
+            float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf);
+            if ( !isnan(tInterp) && !isinf(tInterp) )
+            {
+                float3 pvox = origScaled + tInterp * dirScaled;
+                float3 nv = getNormalVoxel( pvox, allVolumePtr, volumeUnitDegree,
+                                            hash_divisor, hashes, data,
+                                            volStrides, table_offset);
+
+                if(!any(isnan(nv)))
+                {
+                    //convert pv and nv to camera space
+                    normal = (float3)(dot(nv, volRot0),
+                                      dot(nv, volRot1),
+                                      dot(nv, volRot2));
+                    // interpolation optimized a little
+                    float3 pv = pvox * voxelSize;
+                    point = (float3)(dot(pv, volRot0),
+                                     dot(pv, volRot1),
+                                     dot(pv, volRot2)) + volTrans;
+                }
+            }
+            break;
+        }
+        prevTsdf = currTsdf;
+        tprev = tcurr;
+        tcurr += stepSize;
+    }
+
+    __global float* pts = (__global float*)(pointsptr  +  points_offset + y*points_step   + x*sizeof(ptype));
+    __global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step  + x*sizeof(ptype));
+    vstore4((float4)(point,  0), 0, pts);
+    vstore4((float4)(normal, 0), 0, nrm);
+}
+
+
+__kernel void markActive (
+        __global const int4* hashSetData,
+
+        __global char* isActiveFlagsPtr,
+        int isActiveFlagsStep, int isActiveFlagsOffset,
+        int isActiveFlagsRows, int isActiveFlagsCols,
+
+        __global char* lastVisibleIndicesPtr,
+        int lastVisibleIndicesStep, int lastVisibleIndicesOffset,
+        int lastVisibleIndicesRows, int lastVisibleIndicesCols,
+
+        const float16 vol2cam,
+        const float2 fxy,
+        const float2 cxy,
+        const int2 frameSz,
+        const float volumeUnitSize,
+        const int lastVolIndex,
+        const float truncateThreshold,
+        const int frameId
+        )
+{
+    const int hash_divisor = HASH_DIVISOR;
+    int row = get_global_id(0);
+
+    if (row < lastVolIndex)
+    {
+        int3 idx = hashSetData[row].xyz;
+
+        float3 volumeUnitPos = convert_float3(idx) * volumeUnitSize;
+
+        float3 volUnitInCamSpace = (float3) (dot(volumeUnitPos, vol2cam.s012),
+                                             dot(volumeUnitPos, vol2cam.s456),
+                                             dot(volumeUnitPos, vol2cam.s89a)) + vol2cam.s37b;
+
+        if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold)
+        {
+            *(isActiveFlagsPtr + isActiveFlagsOffset + row * isActiveFlagsStep) = 0;
+            return;
+        }
+
+        float2 cameraPoint;
+        float invz = 1.f / volUnitInCamSpace.z;
+        cameraPoint = fxy * volUnitInCamSpace.xy * invz + cxy;
+
+        if (all(cameraPoint >= 0) && all(cameraPoint < convert_float2(frameSz)))
+        {
+            *(__global int*)(lastVisibleIndicesPtr + lastVisibleIndicesOffset + row * lastVisibleIndicesStep) = frameId;
+            *(isActiveFlagsPtr + isActiveFlagsOffset + row * isActiveFlagsStep) = 1;
+        }
+    }
+}

modules/ptcloud/src/opencl/icp.cl

@@ -0,0 +1,239 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+#define UTSIZE 27
+
+typedef float4 ptype;
+
+/*
+    Calculate an upper triangle of Ab matrix then reduce it across workgroup
+*/
+
+inline void calcAb7(__global const char * oldPointsptr,
+                    int oldPoints_step, int oldPoints_offset,
+                    __global const char * oldNormalsptr,
+                    int oldNormals_step, int oldNormals_offset,
+                    const int2 oldSize,
+                    __global const char * newPointsptr,
+                    int newPoints_step, int newPoints_offset,
+                    __global const char * newNormalsptr,
+                    int newNormals_step, int newNormals_offset,
+                    const int2 newSize,
+                    const float16 poseMatrix,
+                    const float2 fxy,
+                    const float2 cxy,
+                    const float sqDistanceThresh,
+                    const float minCos,
+                    float* ab7
+                    )
+{
+    const int x = get_global_id(0);
+    const int y = get_global_id(1);
+
+    if(x >= newSize.x || y >= newSize.y)
+        return;
+
+    // coord-independent constants
+
+    const float3 poseRot0 = poseMatrix.s012;
+    const float3 poseRot1 = poseMatrix.s456;
+    const float3 poseRot2 = poseMatrix.s89a;
+    const float3 poseTrans = poseMatrix.s37b;
+
+    const float2 oldEdge = (float2)(oldSize.x - 1, oldSize.y - 1);
+
+    __global const ptype* newPtsRow = (__global const ptype*)(newPointsptr +
+                                                              newPoints_offset +
+                                                              y*newPoints_step);
+
+    __global const ptype* newNrmRow = (__global const ptype*)(newNormalsptr +
+                                                              newNormals_offset +
+                                                              y*newNormals_step);
+
+    float3 newP = newPtsRow[x].xyz;
+    float3 newN = newNrmRow[x].xyz;
+
+    if( any(isnan(newP)) || any(isnan(newN)) ||
+        any(isinf(newP)) || any(isinf(newN)) )
+        return;
+
+    //transform to old coord system
+    newP = (float3)(dot(newP, poseRot0),
+                    dot(newP, poseRot1),
+                    dot(newP, poseRot2)) + poseTrans;
+    newN = (float3)(dot(newN, poseRot0),
+                    dot(newN, poseRot1),
+                    dot(newN, poseRot2));
+
+    //find correspondence by projecting the point
+    float2 oldCoords = (newP.xy/newP.z)*fxy+cxy;
+
+    if(!(all(oldCoords >= 0.f) && all(oldCoords < oldEdge)))
+        return;
+
+    // bilinearly interpolate oldPts and oldNrm under oldCoords point
+    float3 oldP, oldN;
+    float2 ip = floor(oldCoords);
+    float2 t = oldCoords - ip;
+    int xi = ip.x, yi = ip.y;
+
+    __global const ptype* prow0 = (__global const ptype*)(oldPointsptr +
+                                                          oldPoints_offset +
+                                                          (yi+0)*oldPoints_step);
+    __global const ptype* prow1 = (__global const ptype*)(oldPointsptr +
+                                                          oldPoints_offset +
+                                                          (yi+1)*oldPoints_step);
+    float3 p00 = prow0[xi+0].xyz;
+    float3 p01 = prow0[xi+1].xyz;
+    float3 p10 = prow1[xi+0].xyz;
+    float3 p11 = prow1[xi+1].xyz;
+
+    // NaN check is done later
+
+    __global const ptype* nrow0 = (__global const ptype*)(oldNormalsptr +
+                                                          oldNormals_offset +
+                                                          (yi+0)*oldNormals_step);
+    __global const ptype* nrow1 = (__global const ptype*)(oldNormalsptr +
+                                                          oldNormals_offset +
+                                                          (yi+1)*oldNormals_step);
+
+    float3 n00 = nrow0[xi+0].xyz;
+    float3 n01 = nrow0[xi+1].xyz;
+    float3 n10 = nrow1[xi+0].xyz;
+    float3 n11 = nrow1[xi+1].xyz;
+
+    // NaN check is done later
+
+    float3 p0 = mix(p00, p01, t.x);
+    float3 p1 = mix(p10, p11, t.x);
+    oldP = mix(p0, p1, t.y);
+
+    float3 n0 = mix(n00, n01, t.x);
+    float3 n1 = mix(n10, n11, t.x);
+    oldN = mix(n0, n1, t.y);
+
+    if( any(isnan(oldP)) || any(isnan(oldN)) ||
+        any(isinf(oldP)) || any(isinf(oldN)) )
+        return;
+
+    //filter by distance
+    float3 diff = newP - oldP;
+    if(dot(diff, diff) > sqDistanceThresh)
+        return;
+
+    //filter by angle
+    if(fabs(dot(newN, oldN)) < minCos)
+        return;
+
+    // build point-wise vector ab = [ A | b ]
+
+    float3 VxN = cross(newP, oldN);
+    float ab[7] = {VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, -dot(oldN, diff)};
+
+    for(int i = 0; i < 7; i++)
+        ab7[i] = ab[i];
+}
+
+
+__kernel void getAb(__global const char * oldPointsptr,
+                    int oldPoints_step, int oldPoints_offset,
+                    __global const char * oldNormalsptr,
+                    int oldNormals_step, int oldNormals_offset,
+                    const int2 oldSize,
+                    __global const char * newPointsptr,
+                    int newPoints_step, int newPoints_offset,
+                    __global const char * newNormalsptr,
+                    int newNormals_step, int newNormals_offset,
+                    const int2 newSize,
+                    const float16 poseMatrix,
+                    const float2 fxy,
+                    const float2 cxy,
+                    const float sqDistanceThresh,
+                    const float minCos,
+                    __local float * reducebuf,
+                    __global char* groupedSumptr,
+                    int groupedSum_step, int groupedSum_offset
+)
+{
+    const int x = get_global_id(0);
+    const int y = get_global_id(1);
+
+    const int gx = get_group_id(0);
+    const int gy = get_group_id(1);
+    const int gw = get_num_groups(0);
+    const int gh = get_num_groups(1);
+
+    const int lx = get_local_id(0);
+    const int ly = get_local_id(1);
+    const int lw = get_local_size(0);
+    const int lh = get_local_size(1);
+    const int lsz = lw*lh;
+    const int lid = lx + ly*lw;
+
+    float ab[7];
+    for(int i = 0; i < 7; i++)
+        ab[i] = 0;
+
+    calcAb7(oldPointsptr,
+            oldPoints_step, oldPoints_offset,
+            oldNormalsptr,
+            oldNormals_step, oldNormals_offset,
+            oldSize,
+            newPointsptr,
+            newPoints_step, newPoints_offset,
+            newNormalsptr,
+            newNormals_step, newNormals_offset,
+            newSize,
+            poseMatrix,
+            fxy, cxy,
+            sqDistanceThresh,
+            minCos,
+            ab);
+
+    // build point-wise upper-triangle matrix [ab^T * ab] w/o last row
+    // which is [A^T*A | A^T*b]
+    // and gather sum
+
+    __local float* upperTriangle = reducebuf + lid*UTSIZE;
+
+    int pos = 0;
+    for(int i = 0; i < 6; i++)
+    {
+        for(int j = i; j < 7; j++)
+        {
+            upperTriangle[pos++] = ab[i]*ab[j];
+        }
+    }
+
+    // reduce upperTriangle to local mem
+
+    // maxStep = ctz(lsz), ctz isn't supported on CUDA devices
+    const int c = clz(lsz & -lsz);
+    const int maxStep = c ? 31 - c : c;
+    for(int nstep = 1; nstep <= maxStep; nstep++)
+    {
+        if(lid % (1 << nstep) == 0)
+        {
+            __local float* rto   = reducebuf + UTSIZE*lid;
+            __local float* rfrom = reducebuf + UTSIZE*(lid+(1 << (nstep-1)));
+            for(int i = 0; i < UTSIZE; i++)
+                rto[i] += rfrom[i];
+        }
+        barrier(CLK_LOCAL_MEM_FENCE);
+    }
+
+    // here group sum should be in reducebuf[0...UTSIZE]
+    if(lid == 0)
+    {
+        __global float* groupedRow = (__global float*)(groupedSumptr +
+                                                       groupedSum_offset +
+                                                       gy*groupedSum_step);
+
+        for(int i = 0; i < UTSIZE; i++)
+            groupedRow[gx*UTSIZE + i] = reducebuf[i];
+    }
+}

modules/ptcloud/src/opencl/kinfu_frame.cl

@@ -0,0 +1,288 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+inline float3 reproject(float3 p, float2 fxyinv, float2 cxy)
+{
+    float2 pp = p.z*(p.xy - cxy)*fxyinv;
+    return (float3)(pp, p.z);
+}
+
+typedef float4 ptype;
+
+__kernel void computePointsNormals(__global char * pointsptr,
+                                   int points_step, int points_offset,
+                                   __global char * normalsptr,
+                                   int normals_step, int normals_offset,
+                                   __global const char * depthptr,
+                                   int depth_step, int depth_offset,
+                                   int depth_rows, int depth_cols,
+                                   const float2 fxyinv,
+                                   const float2 cxy,
+                                   const float dfac
+                                    )
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= depth_cols || y >= depth_rows)
+        return;
+
+    __global const float* row0 = (__global const float*)(depthptr + depth_offset +
+                                                         (y+0)*depth_step);
+    __global const float* row1 = (__global const float*)(depthptr + depth_offset +
+                                                         (y+1)*depth_step);
+
+    float d00 = row0[x];
+    float z00 = d00*dfac;
+    float3 p00 = (float3)(convert_float2((int2)(x, y)), z00);
+    float3 v00 = reproject(p00, fxyinv, cxy);
+
+    float3 p = nan((uint)0), n = nan((uint)0);
+
+    if(x < depth_cols - 1 && y < depth_rows - 1)
+    {
+        float d01 = row0[x+1];
+        float d10 = row1[x];
+
+        float z01 = d01*dfac;
+        float z10 = d10*dfac;
+
+        if(z00 != 0 && z01 != 0 && z10 != 0)
+        {
+            float3 p01 = (float3)(convert_float2((int2)(x+1, y+0)), z01);
+            float3 p10 = (float3)(convert_float2((int2)(x+0, y+1)), z10);
+            float3 v01 = reproject(p01, fxyinv, cxy);
+            float3 v10 = reproject(p10, fxyinv, cxy);
+
+            float3 vec = cross(v01 - v00, v10 - v00);
+            n = - normalize(vec);
+            p = v00;
+        }
+    }
+
+    __global float* pts = (__global float*)(pointsptr  +  points_offset + y*points_step  + x*sizeof(ptype));
+    __global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype));
+    vstore4((ptype)(p, 0), 0, pts);
+    vstore4((ptype)(n, 0), 0, nrm);
+}
+
+__kernel void pyrDownBilateral(__global const char * depthptr,
+                               int depth_step, int depth_offset,
+                               int depth_rows, int depth_cols,
+                               __global char * depthDownptr,
+                               int depthDown_step, int depthDown_offset,
+                               int depthDown_rows, int depthDown_cols,
+                               const float sigma
+                               )
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= depthDown_cols || y >= depthDown_rows)
+        return;
+
+    const float sigma3 = sigma*3;
+    const int D = 5;
+
+    __global const float* srcCenterRow = (__global const float*)(depthptr + depth_offset +
+                                                                 (2*y)*depth_step);
+
+    float center = srcCenterRow[2*x];
+
+    int sx = max(0, 2*x - D/2), ex = min(2*x - D/2 + D, depth_cols-1);
+    int sy = max(0, 2*y - D/2), ey = min(2*y - D/2 + D, depth_rows-1);
+
+    float sum = 0;
+    int count = 0;
+
+    for(int iy = sy; iy < ey; iy++)
+    {
+        __global const float* srcRow = (__global const float*)(depthptr + depth_offset +
+                                                               (iy)*depth_step);
+        for(int ix = sx; ix < ex; ix++)
+        {
+            float val = srcRow[ix];
+            if(fabs(val - center) < sigma3)
+            {
+                sum += val; count++;
+            }
+        }
+    }
+
+    __global float* downRow = (__global float*)(depthDownptr + depthDown_offset +
+                                                y*depthDown_step + x*sizeof(float));
+
+    *downRow = (count == 0) ? 0 : sum/convert_float(count);
+}
+
+//TODO: remove bilateral when OpenCV performs 32f bilat with OpenCL
+
+__kernel void customBilateral(__global const char * srcptr,
+                              int src_step, int src_offset,
+                              __global char * dstptr,
+                              int dst_step, int dst_offset,
+                              const int2 frameSize,
+                              const int kernelSize,
+                              const float sigma_spatial2_inv_half,
+                              const float sigma_depth2_inv_half
+)
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= frameSize.x || y >= frameSize.y)
+        return;
+
+    __global const float* srcCenterRow = (__global const float*)(srcptr + src_offset +
+                                                                 y*src_step);
+    float value = srcCenterRow[x];
+
+    int tx = min (x - kernelSize / 2 + kernelSize, frameSize.x - 1);
+    int ty = min (y - kernelSize / 2 + kernelSize, frameSize.y - 1);
+
+    float sum1 = 0;
+    float sum2 = 0;
+
+    for (int cy = max (y - kernelSize / 2, 0); cy < ty; ++cy)
+    {
+        __global const float* srcRow = (__global const float*)(srcptr + src_offset +
+                                                               cy*src_step);
+        for (int cx = max (x - kernelSize / 2, 0); cx < tx; ++cx)
+        {
+            float depth = srcRow[cx];
+
+            float space2 = convert_float((x - cx) * (x - cx) + (y - cy) * (y - cy));
+            float color2 = (value - depth) * (value - depth);
+
+            float weight = native_exp (-(space2 * sigma_spatial2_inv_half +
+                                         color2 * sigma_depth2_inv_half));
+
+            sum1 += depth * weight;
+            sum2 += weight;
+        }
+    }
+
+    __global float* dst = (__global float*)(dstptr + dst_offset +
+                                            y*dst_step + x*sizeof(float));
+    *dst = sum1/sum2;
+}
+
+__kernel void pyrDownPointsNormals(__global const char * pptr,
+                                   int p_step, int p_offset,
+                                   __global const char * nptr,
+                                   int n_step, int n_offset,
+                                   __global char * pdownptr,
+                                   int pdown_step, int pdown_offset,
+                                   __global char * ndownptr,
+                                   int ndown_step, int ndown_offset,
+                                   const int2 downSize
+                                   )
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= downSize.x || y >= downSize.y)
+        return;
+
+    float3 point = nan((uint)0), normal = nan((uint)0);
+
+    __global const ptype* pUpRow0 = (__global const ptype*)(pptr + p_offset + (2*y  )*p_step);
+    __global const ptype* pUpRow1 = (__global const ptype*)(pptr + p_offset + (2*y+1)*p_step);
+
+    float3 d00 = pUpRow0[2*x  ].xyz;
+    float3 d01 = pUpRow0[2*x+1].xyz;
+    float3 d10 = pUpRow1[2*x  ].xyz;
+    float3 d11 = pUpRow1[2*x+1].xyz;
+
+    if(!(any(isnan(d00)) || any(isnan(d01)) ||
+         any(isnan(d10)) || any(isnan(d11))))
+    {
+        point = (d00 + d01 + d10 + d11)*0.25f;
+
+        __global const ptype* nUpRow0 = (__global const ptype*)(nptr + n_offset + (2*y  )*n_step);
+        __global const ptype* nUpRow1 = (__global const ptype*)(nptr + n_offset + (2*y+1)*n_step);
+
+        float3 n00 = nUpRow0[2*x  ].xyz;
+        float3 n01 = nUpRow0[2*x+1].xyz;
+        float3 n10 = nUpRow1[2*x  ].xyz;
+        float3 n11 = nUpRow1[2*x+1].xyz;
+
+        normal = (n00 + n01 + n10 + n11)*0.25f;
+    }
+
+    __global ptype* pts = (__global ptype*)(pdownptr + pdown_offset + y*pdown_step);
+    __global ptype* nrm = (__global ptype*)(ndownptr + ndown_offset + y*ndown_step);
+    pts[x] = (ptype)(point,  0);
+    nrm[x] = (ptype)(normal, 0);
+}
+
+typedef char4 pixelType;
+
+// 20 is fixed power
+float specPow20(float x)
+{
+    float x2 = x*x;
+    float x5 = x2*x2*x;
+    float x10 = x5*x5;
+    float x20 = x10*x10;
+    return x20;
+}
+
+__kernel void render(__global const char * pointsptr,
+                     int points_step, int points_offset,
+                     __global const char * normalsptr,
+                     int normals_step, int normals_offset,
+                     __global char * imgptr,
+                     int img_step, int img_offset,
+                     const int2 frameSize,
+                     const float4 lightPt
+                    )
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= frameSize.x || y >= frameSize.y)
+        return;
+
+    __global const ptype* ptsRow = (__global const ptype*)(pointsptr  + points_offset  + y*points_step  + x*sizeof(ptype));
+    __global const ptype* nrmRow = (__global const ptype*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype));
+
+    float3 p = (*ptsRow).xyz;
+    float3 n = (*nrmRow).xyz;
+
+    pixelType color;
+
+    if(any(isnan(p)))
+    {
+        color = (pixelType)(0, 32, 0, 0);
+    }
+    else
+    {
+        const float Ka = 0.3f;  //ambient coeff
+        const float Kd = 0.5f;  //diffuse coeff
+        const float Ks = 0.2f;  //specular coeff
+        //const int   sp = 20;  //specular power, fixed in specPow20()
+
+        const float Ax = 1.f;   //ambient color,  can be RGB
+        const float Dx = 1.f;   //diffuse color,  can be RGB
+        const float Sx = 1.f;   //specular color, can be RGB
+        const float Lx = 1.f;   //light color
+
+        float3 l = normalize(lightPt.xyz - p);
+        float3 v = normalize(-p);
+        float3 r = normalize(2.f*n*dot(n, l) - l);
+
+        float val = (Ax*Ka*Dx + Lx*Kd*Dx*max(0.f, dot(n, l)) +
+                     Lx*Ks*Sx*specPow20(max(0.f, dot(r, v))));
+
+        uchar ix = convert_uchar(val*255.f);
+        color = (pixelType)(ix, ix, ix, 0);
+    }
+
+    __global char* imgRow = (__global char*)(imgptr + img_offset + y*img_step + x*sizeof(pixelType));
+    vstore4(color, 0, imgRow);
+}

modules/ptcloud/src/opencl/tsdf.cl

@@ -0,0 +1,856 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+typedef char int8_t;
+typedef int8_t TsdfType;
+typedef uchar WeightType;
+
+struct TsdfVoxel
+{
+    TsdfType tsdf;
+    WeightType weight;
+};
+
+static inline TsdfType floatToTsdf(float num)
+{
+    int8_t res = (int8_t) ( (num * (-128)) );
+    res = res ? res : (num < 0 ? 1 : -1);
+    return res;
+}
+
+static inline float tsdfToFloat(TsdfType num)
+{
+    return ( (float) num ) / (-128);
+}
+
+__kernel void integrate(__global const char * depthptr,
+                        int depth_step, int depth_offset,
+                        int depth_rows, int depth_cols,
+                        __global struct TsdfVoxel * volumeptr,
+                        __global const float * vol2camptr,
+                        const float voxelSize,
+                        const int4 volResolution4,
+                        const int4 volDims4,
+                        const float2 fxy,
+                        const float2 cxy,
+                        const float dfac,
+                        const float truncDist,
+                        const int maxWeight,
+                        const __global float * pixNorms)
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    const int3 volResolution = volResolution4.xyz;
+
+    if(x >= volResolution.x || y >= volResolution.y)
+        return;
+
+    // coord-independent constants
+    const int3 volDims = volDims4.xyz;
+    const float2 limits = (float2)(depth_cols-1, depth_rows-1);
+
+    __global const float* vm = vol2camptr;
+    const float4 vol2cam0 = vload4(0, vm);
+    const float4 vol2cam1 = vload4(1, vm);
+    const float4 vol2cam2 = vload4(2, vm);
+
+    const float truncDistInv = 1.f/truncDist;
+
+    // optimization of camSpace transformation (vector addition instead of matmul at each z)
+    float4 inPt = (float4)(x*voxelSize, y*voxelSize, 0, 1);
+    float3 basePt = (float3)(dot(vol2cam0, inPt),
+                             dot(vol2cam1, inPt),
+                             dot(vol2cam2, inPt));
+
+    float3 camSpacePt = basePt;
+
+    // zStep == vol2cam*(float3(x, y, 1)*voxelSize) - basePt;
+    float3 zStep = ((float3)(vol2cam0.z, vol2cam1.z, vol2cam2.z))*voxelSize;
+
+    int volYidx = x*volDims.x + y*volDims.y;
+
+    int startZ, endZ;
+    if(fabs(zStep.z) > 1e-5f)
+    {
+        int baseZ = convert_int(-basePt.z / zStep.z);
+        if(zStep.z > 0)
+        {
+            startZ = baseZ;
+            endZ = volResolution.z;
+        }
+        else
+        {
+            startZ = 0;
+            endZ = baseZ;
+        }
+    }
+    else
+    {
+        if(basePt.z > 0)
+        {
+            startZ = 0; endZ = volResolution.z;
+        }
+        else
+        {
+            // z loop shouldn't be performed
+            //startZ = endZ = 0;
+            return;
+        }
+    }
+
+    startZ = max(0, startZ);
+    endZ = min(volResolution.z, endZ);
+
+    for(int z = startZ; z < endZ; z++)
+    {
+        // optimization of the following:
+        //float3 camSpacePt = vol2cam * ((float3)(x, y, z)*voxelSize);
+        camSpacePt += zStep;
+
+        if(camSpacePt.z <= 0)
+            continue;
+
+        float3 camPixVec = camSpacePt / camSpacePt.z;
+        float2 projected = mad(camPixVec.xy, fxy, cxy);
+
+        float v;
+        // bilinearly interpolate depth at projected
+        if(all(projected >= 0) && all(projected < limits))
+        {
+            float2 ip = floor(projected);
+            int xi = ip.x, yi = ip.y;
+
+            __global const float* row0 = (__global const float*)(depthptr + depth_offset +
+                                                                 (yi+0)*depth_step);
+            __global const float* row1 = (__global const float*)(depthptr + depth_offset +
+                                                                 (yi+1)*depth_step);
+
+            float v00 = row0[xi+0];
+            float v01 = row0[xi+1];
+            float v10 = row1[xi+0];
+            float v11 = row1[xi+1];
+            float4 vv = (float4)(v00, v01, v10, v11);
+
+            // assume correct depth is positive
+            if(all(vv > 0))
+            {
+                float2 t = projected - ip;
+                float2 vf = mix(vv.xz, vv.yw, t.x);
+                v = mix(vf.s0, vf.s1, t.y);
+            }
+            else
+                continue;
+        }
+        else
+            continue;
+
+        if(v == 0)
+            continue;
+
+        int idx = projected.y * depth_cols + projected.x;
+        float pixNorm = pixNorms[idx];
+        //float pixNorm = length(camPixVec);
+
+        // difference between distances of point and of surface to camera
+        float sdf = pixNorm*(v*dfac - camSpacePt.z);
+        // possible alternative is:
+        // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0);
+
+        if(sdf >= -truncDist)
+        {
+            float tsdf = fmin(1.0f, sdf * truncDistInv);
+            int volIdx = volYidx + z*volDims.z;
+
+            struct TsdfVoxel voxel = volumeptr[volIdx];
+            float value  = tsdfToFloat(voxel.tsdf);
+            int weight = voxel.weight;
+
+            // update TSDF
+            value = (value*weight + tsdf) / (weight + 1);
+            weight = min(weight + 1, maxWeight);
+
+            voxel.tsdf = floatToTsdf(value);
+            voxel.weight = weight;
+            volumeptr[volIdx] = voxel;
+        }
+    }
+}
+
+
+inline float interpolateVoxel(float3 p, __global const struct TsdfVoxel* volumePtr,
+                              int3 volDims, int8 neighbourCoords)
+{
+    float3 fip = floor(p);
+    int3 ip = convert_int3(fip);
+    float3 t = p - fip;
+
+    int3 cmul = volDims*ip;
+    int coordBase = cmul.x + cmul.y + cmul.z;
+    int nco[8];
+    vstore8(neighbourCoords + coordBase, 0, nco);
+
+    float vaz[8];
+    for(int i = 0; i < 8; i++)
+        vaz[i] = tsdfToFloat(volumePtr[nco[i]].tsdf);
+
+    float8 vz = vload8(0, vaz);
+
+    float4 vy = mix(vz.s0246, vz.s1357, t.z);
+    float2 vx = mix(vy.s02, vy.s13, t.y);
+    return mix(vx.s0, vx.s1, t.x);
+}
+
+inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePtr,
+                             int3 volResolution, int3 volDims, int8 neighbourCoords)
+{
+    if(any(p < 1) || any(p >= convert_float3(volResolution - 2)))
+        return nan((uint)0);
+
+    float3 fip = floor(p);
+    int3 ip = convert_int3(fip);
+    float3 t = p - fip;
+
+    int3 cmul = volDims*ip;
+    int coordBase = cmul.x + cmul.y + cmul.z;
+    int nco[8];
+    vstore8(neighbourCoords + coordBase, 0, nco);
+
+    int arDims[3];
+    vstore3(volDims, 0, arDims);
+    float an[3];
+    for(int c = 0; c < 3; c++)
+    {
+        int dim = arDims[c];
+
+        float vaz[8];
+        for(int i = 0; i < 8; i++)
+            vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf) -
+                     tsdfToFloat(volumePtr[nco[i] - dim].tsdf);
+
+        float8 vz = vload8(0, vaz);
+
+        float4 vy = mix(vz.s0246, vz.s1357, t.z);
+        float2 vx = mix(vy.s02, vy.s13, t.y);
+
+        an[c] = mix(vx.s0, vx.s1, t.x);
+    }
+
+    //gradientDeltaFactor is fixed at 1.0 of voxel size
+    float3 n = vload3(0, an);
+    float Norm = sqrt(n.x*n.x + n.y*n.y + n.z*n.z);
+    return Norm < 0.0001f ? nan((uint)0) : n / Norm;
+    //return fast_normalize(vload3(0, an));
+}
+
+typedef float4 ptype;
+
+__kernel void raycast(__global char * pointsptr,
+                      int points_step, int points_offset,
+                      __global char * normalsptr,
+                      int normals_step, int normals_offset,
+                      const int2 frameSize,
+                      __global const struct TsdfVoxel * volumeptr,
+                      __global const float * vol2camptr,
+                      __global const float * cam2volptr,
+                      const float2 fixy,
+                      const float2 cxy,
+                      const float4 boxDown4,
+                      const float4 boxUp4,
+                      const float tstep,
+                      const float voxelSize,
+                      const int4 volResolution4,
+                      const int4 volDims4,
+                      const int8 neighbourCoords
+                      )
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= frameSize.x || y >= frameSize.y)
+        return;
+
+    // coordinate-independent constants
+
+    __global const float* cm = cam2volptr;
+    const float3 camRot0  = vload4(0, cm).xyz;
+    const float3 camRot1  = vload4(1, cm).xyz;
+    const float3 camRot2  = vload4(2, cm).xyz;
+    const float3 camTrans = (float3)(cm[3], cm[7], cm[11]);
+
+    __global const float* vm = vol2camptr;
+    const float3 volRot0  = vload4(0, vm).xyz;
+    const float3 volRot1  = vload4(1, vm).xyz;
+    const float3 volRot2  = vload4(2, vm).xyz;
+    const float3 volTrans = (float3)(vm[3], vm[7], vm[11]);
+
+    const float3 boxDown = boxDown4.xyz;
+    const float3 boxUp   = boxUp4.xyz;
+    const int3   volDims = volDims4.xyz;
+
+    const int3 volResolution = volResolution4.xyz;
+
+    const float invVoxelSize = native_recip(voxelSize);
+
+    // kernel itself
+
+    float3 point  = nan((uint)0);
+    float3 normal = nan((uint)0);
+
+    float3 orig = camTrans;
+
+    // get direction through pixel in volume space:
+    // 1. reproject (x, y) on projecting plane where z = 1.f
+    float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f);
+
+    // 2. rotate to volume space
+    planed = (float3)(dot(planed, camRot0),
+                      dot(planed, camRot1),
+                      dot(planed, camRot2));
+
+    // 3. normalize
+    float3 dir = fast_normalize(planed);
+
+    // compute intersection of ray with all six bbox planes
+    float3 rayinv = native_recip(dir);
+    float3 tbottom = rayinv*(boxDown - orig);
+    float3 ttop    = rayinv*(boxUp   - orig);
+
+    // re-order intersections to find smallest and largest on each axis
+    float3 minAx = min(ttop, tbottom);
+    float3 maxAx = max(ttop, tbottom);
+
+    // near clipping plane
+    const float clip = 0.f;
+    float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip);
+    float tmax =     min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z));
+
+    // precautions against getting coordinates out of bounds
+    tmin = tmin + tstep;
+    tmax = tmax - tstep;
+
+    if(tmin < tmax)
+    {
+        // interpolation optimized a little
+        orig *= invVoxelSize;
+        dir  *= invVoxelSize;
+
+        float3 rayStep = dir*tstep;
+        float3 next = (orig + dir*tmin);
+        float f = interpolateVoxel(next, volumeptr, volDims, neighbourCoords);
+        float fnext = f;
+
+        // raymarch
+        int steps = 0;
+        int nSteps = floor(native_divide(tmax - tmin, tstep));
+        bool stop = false;
+        for(int i = 0; i < nSteps; i++)
+        {
+            // fix for wrong steps counting
+            if(!stop)
+            {
+                next += rayStep;
+
+                // fetch voxel
+                int3 ip = convert_int3(round(next));
+                int3 cmul = ip*volDims;
+                int idx = cmul.x + cmul.y + cmul.z;
+                fnext = tsdfToFloat(volumeptr[idx].tsdf);
+
+                if(fnext != f)
+                {
+                    fnext = interpolateVoxel(next, volumeptr, volDims, neighbourCoords);
+
+                    // when ray crosses a surface
+                    if(signbit(f) != signbit(fnext))
+                    {
+                        stop = true; continue;
+                    }
+
+                    f = fnext;
+                }
+                steps++;
+            }
+        }
+
+        // if ray penetrates a surface from outside
+        // linearly interpolate t between two f values
+        if(f > 0 && fnext < 0)
+        {
+            float3 tp = next - rayStep;
+            float ft   = interpolateVoxel(tp,   volumeptr, volDims, neighbourCoords);
+            float ftdt = interpolateVoxel(next, volumeptr, volDims, neighbourCoords);
+            // float t = tmin + steps*tstep;
+            // float ts = t - tstep*ft/(ftdt - ft);
+            float ts = tmin + tstep*(steps - native_divide(ft, ftdt - ft));
+
+            // avoid division by zero
+            if(!isnan(ts) && !isinf(ts))
+            {
+                float3 pv = orig + dir*ts;
+                float3 nv = getNormalVoxel(pv, volumeptr, volResolution, volDims, neighbourCoords);
+
+                if(!any(isnan(nv)))
+                {
+                    //convert pv and nv to camera space
+                    normal = (float3)(dot(nv, volRot0),
+                                      dot(nv, volRot1),
+                                      dot(nv, volRot2));
+                    // interpolation optimized a little
+                    pv *= voxelSize;
+                    point = (float3)(dot(pv, volRot0),
+                                     dot(pv, volRot1),
+                                     dot(pv, volRot2)) + volTrans;
+                }
+            }
+        }
+    }
+
+    __global float* pts = (__global float*)(pointsptr  +  points_offset + y*points_step  + x*sizeof(ptype));
+    __global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype));
+    vstore4((float4)(point,  0), 0, pts);
+    vstore4((float4)(normal, 0), 0, nrm);
+}
+
+
+__kernel void getNormals(__global const char * pointsptr,
+                         int points_step, int points_offset,
+                         __global char * normalsptr,
+                         int normals_step, int normals_offset,
+                         const int2 frameSize,
+                         __global const struct TsdfVoxel* volumeptr,
+                         __global const float * volPoseptr,
+                         __global const float * invPoseptr,
+                         const float voxelSizeInv,
+                         const int4 volResolution4,
+                         const int4 volDims4,
+                         const int8 neighbourCoords
+                         )
+{
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+
+    if(x >= frameSize.x || y >= frameSize.y)
+        return;
+
+    // coordinate-independent constants
+
+    __global const float* vp = volPoseptr;
+    const float3 volRot0  = vload4(0, vp).xyz;
+    const float3 volRot1  = vload4(1, vp).xyz;
+    const float3 volRot2  = vload4(2, vp).xyz;
+    const float3 volTrans = (float3)(vp[3], vp[7], vp[11]);
+
+    __global const float* iv = invPoseptr;
+    const float3 invRot0 = vload4(0, iv).xyz;
+    const float3 invRot1 = vload4(1, iv).xyz;
+    const float3 invRot2 = vload4(2, iv).xyz;
+    const float3 invTrans = (float3)(iv[3], iv[7], iv[11]);
+
+    const int3 volResolution = volResolution4.xyz;
+    const int3 volDims = volDims4.xyz;
+
+    // kernel itself
+
+    __global const ptype* ptsRow = (__global const ptype*)(pointsptr +
+                                                           points_offset +
+                                                           y*points_step);
+    float3 p = ptsRow[x].xyz;
+    float3 n = nan((uint)0);
+    if(!any(isnan(p)))
+    {
+        float3 voxPt = (float3)(dot(p, invRot0),
+                                dot(p, invRot1),
+                                dot(p, invRot2)) + invTrans;
+        voxPt = voxPt * voxelSizeInv;
+        n = getNormalVoxel(voxPt, volumeptr, volResolution, volDims, neighbourCoords);
+        n = (float3)(dot(n, volRot0),
+                     dot(n, volRot1),
+                     dot(n, volRot2));
+    }
+
+    __global float* nrm = (__global float*)(normalsptr +
+                                            normals_offset +
+                                            y*normals_step +
+                                            x*sizeof(ptype));
+
+    vstore4((float4)(n, 0), 0, nrm);
+}
+
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics:enable
+
+struct CoordReturn
+{
+    bool result;
+    float3 point;
+    float3 normal;
+};
+
+inline struct CoordReturn coord(int x, int y, int z, float3 V, float v0, int axis,
+                                __global const struct TsdfVoxel* volumeptr,
+                                int3 volResolution, int3 volDims,
+                                int8 neighbourCoords,
+                                float voxelSize, float voxelSizeInv,
+                                const float3 volRot0,
+                                const float3 volRot1,
+                                const float3 volRot2,
+                                const float3 volTrans,
+                                bool needNormals,
+                                bool scan
+                                )
+{
+    struct CoordReturn cr;
+
+    // 0 for x, 1 for y, 2 for z
+    bool limits = false;
+    int3 shift;
+    float Vc = 0.f;
+    if(axis == 0)
+    {
+        shift = (int3)(1, 0, 0);
+        limits = (x + 1 < volResolution.x);
+        Vc = V.x;
+    }
+    if(axis == 1)
+    {
+        shift = (int3)(0, 1, 0);
+        limits = (y + 1 < volResolution.y);
+        Vc = V.y;
+    }
+    if(axis == 2)
+    {
+        shift = (int3)(0, 0, 1);
+        limits = (z + 1 < volResolution.z);
+        Vc = V.z;
+    }
+
+    if(limits)
+    {
+        int3 ip = ((int3)(x, y, z)) + shift;
+        int3 cmul = ip*volDims;
+        int idx = cmul.x + cmul.y + cmul.z;
+
+        struct TsdfVoxel voxel = volumeptr[idx];
+        float vd  = tsdfToFloat(voxel.tsdf);
+        int weight = voxel.weight;
+
+        if(weight != 0 && vd != 1.f)
+        {
+            if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0))
+            {
+                // calc actual values or estimate amount of space
+                if(!scan)
+                {
+                    // linearly interpolate coordinate
+                    float Vn = Vc + voxelSize;
+                    float dinv = 1.f/(fabs(v0)+fabs(vd));
+                    float inter = (Vc*fabs(vd) + Vn*fabs(v0))*dinv;
+
+                    float3 p = (float3)(shift.x ? inter : V.x,
+                                        shift.y ? inter : V.y,
+                                        shift.z ? inter : V.z);
+
+                    cr.point = (float3)(dot(p, volRot0),
+                                        dot(p, volRot1),
+                                        dot(p, volRot2)) + volTrans;
+
+                    if(needNormals)
+                    {
+                        float3 nv = getNormalVoxel(p * voxelSizeInv,
+                                                   volumeptr, volResolution, volDims, neighbourCoords);
+
+                        cr.normal = (float3)(dot(nv, volRot0),
+                                             dot(nv, volRot1),
+                                             dot(nv, volRot2));
+                    }
+                }
+
+                cr.result = true;
+                return cr;
+            }
+        }
+    }
+
+    cr.result = false;
+    return cr;
+}
+
+
+__kernel void scanSize(__global const struct TsdfVoxel* volumeptr,
+                       const int4 volResolution4,
+                       const int4 volDims4,
+                       const int8 neighbourCoords,
+                       __global const float * volPoseptr,
+                       const float voxelSize,
+                       const float voxelSizeInv,
+                       __local int* reducebuf,
+                       __global char* groupedSumptr,
+                       int groupedSum_slicestep,
+                       int groupedSum_step, int groupedSum_offset
+                       )
+{
+    const int3 volDims = volDims4.xyz;
+    const int3 volResolution = volResolution4.xyz;
+
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+    int z = get_global_id(2);
+
+    bool validVoxel = true;
+    if(x >= volResolution.x || y >= volResolution.y || z >= volResolution.z)
+        validVoxel = false;
+
+    const int gx = get_group_id(0);
+    const int gy = get_group_id(1);
+    const int gz = get_group_id(2);
+
+    const int lx = get_local_id(0);
+    const int ly = get_local_id(1);
+    const int lz = get_local_id(2);
+    const int lw = get_local_size(0);
+    const int lh = get_local_size(1);
+    const int ld = get_local_size(2);
+    const int lsz = lw*lh*ld;
+    const int lid = lx + ly*lw + lz*lw*lh;
+
+    // coordinate-independent constants
+
+    __global const float* vp = volPoseptr;
+    const float3 volRot0  = vload4(0, vp).xyz;
+    const float3 volRot1  = vload4(1, vp).xyz;
+    const float3 volRot2  = vload4(2, vp).xyz;
+    const float3 volTrans = (float3)(vp[3], vp[7], vp[11]);
+
+    // kernel itself
+    int npts = 0;
+    if(validVoxel)
+    {
+        int3 ip = (int3)(x, y, z);
+        int3 cmul = ip*volDims;
+        int idx = cmul.x + cmul.y + cmul.z;
+        struct TsdfVoxel voxel = volumeptr[idx];
+        float value  = tsdfToFloat(voxel.tsdf);
+        int weight = voxel.weight;
+
+        // if voxel is not empty
+        if(weight != 0 && value != 1.f)
+        {
+            float3 V = (((float3)(x, y, z)) + 0.5f)*voxelSize;
+
+            #pragma unroll
+            for(int i = 0; i < 3; i++)
+            {
+                struct CoordReturn cr;
+                cr = coord(x, y, z, V, value, i,
+                           volumeptr, volResolution, volDims,
+                           neighbourCoords,
+                           voxelSize, voxelSizeInv,
+                           volRot0, volRot1, volRot2, volTrans,
+                           false, true);
+                if(cr.result)
+                {
+                    npts++;
+                }
+            }
+        }
+    }
+
+    // reducebuf keeps counters for each thread
+    reducebuf[lid] = npts;
+
+    // reduce counter to local mem
+
+    // maxStep = ctz(lsz), ctz isn't supported on CUDA devices
+    const int c = clz(lsz & -lsz);
+    const int maxStep = c ? 31 - c : c;
+    for(int nstep = 1; nstep <= maxStep; nstep++)
+    {
+        if(lid % (1 << nstep) == 0)
+        {
+            int rto   = lid;
+            int rfrom = lid + (1 << (nstep-1));
+            reducebuf[rto] += reducebuf[rfrom];
+        }
+        barrier(CLK_LOCAL_MEM_FENCE);
+    }
+
+    if(lid == 0)
+    {
+        __global int* groupedRow = (__global int*)(groupedSumptr +
+                                                   groupedSum_offset +
+                                                   gy*groupedSum_step +
+                                                   gz*groupedSum_slicestep);
+
+        groupedRow[gx] = reducebuf[0];
+    }
+}
+
+
+__kernel void fillPtsNrm(__global const struct TsdfVoxel* volumeptr,
+                         const int4 volResolution4,
+                         const int4 volDims4,
+                         const int8 neighbourCoords,
+                         __global const float * volPoseptr,
+                         const float voxelSize,
+                         const float voxelSizeInv,
+                         const int needNormals,
+                         __local float* localbuf,
+                         volatile __global int* atomicCtr,
+                         __global const char* groupedSumptr,
+                         int groupedSum_slicestep,
+                         int groupedSum_step, int groupedSum_offset,
+                         __global char * pointsptr,
+                         int points_step, int points_offset,
+                         __global char * normalsptr,
+                         int normals_step, int normals_offset
+                         )
+{
+    const int3 volDims = volDims4.xyz;
+    const int3 volResolution = volResolution4.xyz;
+
+    int x = get_global_id(0);
+    int y = get_global_id(1);
+    int z = get_global_id(2);
+
+    bool validVoxel = true;
+    if(x >= volResolution.x || y >= volResolution.y || z >= volResolution.z)
+        validVoxel = false;
+
+    const int gx = get_group_id(0);
+    const int gy = get_group_id(1);
+    const int gz = get_group_id(2);
+
+    __global int* groupedRow = (__global int*)(groupedSumptr +
+                                               groupedSum_offset +
+                                               gy*groupedSum_step +
+                                               gz*groupedSum_slicestep);
+
+    // this group contains 0 pts, skip it
+    int nptsGroup = groupedRow[gx];
+    if(nptsGroup == 0)
+        return;
+
+    const int lx = get_local_id(0);
+    const int ly = get_local_id(1);
+    const int lz = get_local_id(2);
+    const int lw = get_local_size(0);
+    const int lh = get_local_size(1);
+    const int ld = get_local_size(2);
+    const int lsz = lw*lh*ld;
+    const int lid = lx + ly*lw + lz*lw*lh;
+
+    // coordinate-independent constants
+
+    __global const float* vp = volPoseptr;
+    const float3 volRot0  = vload4(0, vp).xyz;
+    const float3 volRot1  = vload4(1, vp).xyz;
+    const float3 volRot2  = vload4(2, vp).xyz;
+    const float3 volTrans = (float3)(vp[3], vp[7], vp[11]);
+
+    // kernel itself
+    int npts = 0;
+    float3 parr[3], narr[3];
+    if(validVoxel)
+    {
+        int3 ip = (int3)(x, y, z);
+        int3 cmul = ip*volDims;
+        int idx = cmul.x + cmul.y + cmul.z;
+        struct TsdfVoxel voxel = volumeptr[idx];
+        float value  = tsdfToFloat(voxel.tsdf);
+        int weight = voxel.weight;
+
+        // if voxel is not empty
+        if(weight != 0 && value != 1.f)
+        {
+            float3 V = (((float3)(x, y, z)) + 0.5f)*voxelSize;
+
+            #pragma unroll
+            for(int i = 0; i < 3; i++)
+            {
+                struct CoordReturn cr;
+                cr = coord(x, y, z, V, value, i,
+                           volumeptr, volResolution, volDims,
+                           neighbourCoords,
+                           voxelSize, voxelSizeInv,
+                           volRot0, volRot1, volRot2, volTrans,
+                           needNormals, false);
+
+                if(cr.result)
+                {
+                    parr[npts] = cr.point;
+                    narr[npts] = cr.normal;
+                    npts++;
+                }
+            }
+        }
+    }
+
+    // 4 floats per point or normal
+    const int elemStep = 4;
+
+    __local float* normAddr;
+    __local int localCtr;
+    if(lid == 0)
+        localCtr = 0;
+
+    // push all pts (and nrm) from private array to local mem
+    int privateCtr = 0;
+    barrier(CLK_LOCAL_MEM_FENCE);
+    privateCtr = atomic_add(&localCtr, npts);
+    barrier(CLK_LOCAL_MEM_FENCE);
+
+    for(int i = 0; i < npts; i++)
+    {
+        __local float* addr = localbuf + (privateCtr+i)*elemStep;
+        vstore4((float4)(parr[i], 0), 0, addr);
+    }
+
+    if(needNormals)
+    {
+        normAddr = localbuf + localCtr*elemStep;
+
+        for(int i = 0; i < npts; i++)
+        {
+            __local float* addr = normAddr + (privateCtr+i)*elemStep;
+            vstore4((float4)(narr[i], 0), 0, addr);
+        }
+    }
+
+    // debugging purposes
+    if(lid == 0)
+    {
+        if(localCtr != nptsGroup)
+        {
+            printf("!!! fetchPointsNormals result may be incorrect, npts != localCtr at %3d %3d %3d: %3d vs %3d\n",
+                   gx, gy, gz, localCtr, nptsGroup);
+        }
+    }
+
+    // copy local buffer to global mem
+    __local int whereToWrite;
+    if(lid == 0)
+        whereToWrite = atomic_add(atomicCtr, localCtr);
+    barrier(CLK_GLOBAL_MEM_FENCE);
+
+    event_t ev[2];
+    int evn = 0;
+    // points and normals are 1-column matrices
+    __global float* pts = (__global float*)(pointsptr +
+                                            points_offset +
+                                            whereToWrite*points_step);
+    ev[evn++] = async_work_group_copy(pts, localbuf, localCtr*elemStep, 0);
+
+    if(needNormals)
+    {
+        __global float* nrm = (__global float*)(normalsptr +
+                                                normals_offset +
+                                                whereToWrite*normals_step);
+        ev[evn++] = async_work_group_copy(nrm, normAddr, localCtr*elemStep, 0);
+    }
+
+    wait_group_events(evn, ev);
+}

modules/ptcloud/src/plane.cpp

@@ -0,0 +1,586 @@
+// 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 is an implementation of a fast plane detection loosely inspired by
+ * Fast Plane Detection and Polygonalization in noisy 3D Range Images
+ * Jann Poppinga, Narunas Vaskevicius, Andreas Birk, and Kaustubh Pathak
+ * and the follow-up
+ * Fast Plane Detection for SLAM from Noisy Range Images in
+ * Both Structured and Unstructured Environments
+ * Junhao Xiao, Jianhua Zhang and Jianwei Zhang
+ * Houxiang Zhang and Hans Petter Hildre
+ */
+
+#include "precomp.hpp"
+
+namespace cv
+{
+
+/** Structure defining a plane. The notations are from the second paper */
+class PlaneBase
+{
+public:
+    PlaneBase(const Vec3f& m, const Vec3f& n_in, int index) :
+        index_(index),
+        n_(n_in),
+        m_sum_(Vec3f(0, 0, 0)),
+        m_(m),
+        Q_(Matx33f::zeros()),
+        mse_(0),
+        K_(0)
+    {
+        UpdateD();
+    }
+
+    virtual
+        ~PlaneBase()
+    {  }
+
+    /** Compute the distance to the plane. This will be implemented by the children to take into account different
+     * sensor models
+     * @param p_j
+     * @return
+     */
+    virtual float distance(const Vec3f& p_j) const = 0;
+
+    /** The d coefficient in the plane equation ax+by+cz+d = 0
+     * @return
+     */
+    inline float d() const
+    {
+        return d_;
+    }
+
+    /** The normal to the plane
+     * @return the normal to the plane
+     */
+    const Vec3f& n() const
+    {
+        return n_;
+    }
+
+    /** Update the different coefficients of the plane, based on the new statistics
+     */
+    void UpdateParameters()
+    {
+        if (empty())
+            return;
+        m_ = m_sum_ / K_;
+        // Compute C
+        Matx33f C = Q_ - m_sum_ * m_.t();
+
+        // Compute n
+        SVD svd(C);
+        n_ = Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2));
+        mse_ = svd.w.at<float>(2) / K_;
+
+        UpdateD();
+    }
+
+    /** Update the different sum of point and sum of point*point.t()
+     */
+    void  UpdateStatistics(const Vec3f& point, const Matx33f& Q_local)
+    {
+        m_sum_ += point;
+        Q_ += Q_local;
+        ++K_;
+    }
+
+    inline size_t empty() const
+    {
+        return K_ == 0;
+    }
+
+    inline int
+        K() const
+    {
+        return K_;
+    }
+    /** The index of the plane */
+    int index_;
+protected:
+    /** The 4th coefficient in the plane equation ax+by+cz+d = 0 */
+    float d_;
+    /** Normal of the plane */
+    Vec3f n_;
+private:
+    inline void UpdateD()
+    {
+        d_ = -m_.dot(n_);
+    }
+    /** The sum of the points */
+    Vec3f m_sum_;
+    /** The mean of the points */
+    Vec3f m_;
+    /** The sum of pi * pi^\top */
+    Matx33f Q_;
+    /** The different matrices we need to update */
+    Matx33f C_;
+    float mse_;
+    /** the number of points that form the plane */
+    int K_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Basic planar child, with no sensor error model
+ */
+class Plane : public PlaneBase
+{
+public:
+    Plane(const Vec3f& m, const Vec3f& n_in, int index) :
+        PlaneBase(m, n_in, index)
+    { }
+
+    /** The computed distance is perfect in that case
+     * @param p_j the point to compute its distance to
+     * @return
+     */
+    float distance(const Vec3f& p_j) const CV_OVERRIDE
+    {
+        return std::abs(float(p_j.dot(n_) + d_));
+    }
+};
+
+/** Planar child with a quadratic error model
+ */
+class PlaneABC : public PlaneBase
+{
+public:
+    PlaneABC(const Vec3f& m, const Vec3f& n_in, int index, float sensor_error_a, float sensor_error_b, float sensor_error_c) :
+        PlaneBase(m, n_in, index),
+        sensor_error_a_(sensor_error_a),
+        sensor_error_b_(sensor_error_b),
+        sensor_error_c_(sensor_error_c)
+    {
+    }
+
+    /** The distance is now computed by taking the sensor error into account */
+    inline float distance(const Vec3f& p_j) const CV_OVERRIDE
+    {
+        float cst = p_j.dot(n_) + d_;
+        float err = sensor_error_a_ * p_j[2] * p_j[2] + sensor_error_b_ * p_j[2] + sensor_error_c_;
+        if (((cst - n_[2] * err <= 0) && (cst + n_[2] * err >= 0)) || ((cst + n_[2] * err <= 0) && (cst - n_[2] * err >= 0)))
+            return 0;
+        return std::min(std::abs(cst - err), std::abs(cst + err));
+    }
+private:
+    float sensor_error_a_;
+    float sensor_error_b_;
+    float sensor_error_c_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** The PlaneGrid contains statistic about the individual tiles
+ */
+class PlaneGrid
+{
+public:
+    PlaneGrid(const Mat_<Vec4f>& points3d, int block_size) :
+        block_size_(block_size)
+    {
+        // Figure out some dimensions
+        int mini_rows = points3d.rows / block_size;
+        if (points3d.rows % block_size != 0)
+            ++mini_rows;
+
+        int mini_cols = points3d.cols / block_size;
+        if (points3d.cols % block_size != 0)
+            ++mini_cols;
+
+        // Compute all the interesting quantities
+        m_.create(mini_rows, mini_cols);
+        n_.create(mini_rows, mini_cols);
+        Q_.create(points3d.rows, points3d.cols);
+        mse_.create(mini_rows, mini_cols);
+        for (int y = 0; y < mini_rows; ++y)
+            for (int x = 0; x < mini_cols; ++x)
+            {
+                // Update the tiles
+                Matx33f Q = Matx33f::zeros();
+                Vec3f m = Vec3f(0, 0, 0);
+                int K = 0;
+                for (int j = y * block_size; j < std::min((y + 1) * block_size, points3d.rows); ++j)
+                {
+                    const Vec4f* vec = points3d.ptr < Vec4f >(j, x * block_size), * vec_end;
+                    float* pointpointt = reinterpret_cast<float*>(Q_.ptr < Vec<float, 9> >(j, x * block_size));
+                    if (x == mini_cols - 1)
+                        vec_end = points3d.ptr < Vec4f >(j, points3d.cols - 1) + 1;
+                    else
+                        vec_end = vec + block_size;
+                    for (; vec != vec_end; ++vec, pointpointt += 9)
+                    {
+                        if (cvIsNaN(vec->val[0]))
+                            continue;
+                        // Fill point*point.t()
+                        *pointpointt = vec->val[0] * vec->val[0];
+                        *(pointpointt + 1) = vec->val[0] * vec->val[1];
+                        *(pointpointt + 2) = vec->val[0] * vec->val[2];
+                        *(pointpointt + 3) = *(pointpointt + 1);
+                        *(pointpointt + 4) = vec->val[1] * vec->val[1];
+                        *(pointpointt + 5) = vec->val[1] * vec->val[2];
+                        *(pointpointt + 6) = *(pointpointt + 2);
+                        *(pointpointt + 7) = *(pointpointt + 5);
+                        *(pointpointt + 8) = vec->val[2] * vec->val[2];
+
+                        Q += *reinterpret_cast<Matx33f*>(pointpointt);
+                        m += Vec3f((*vec)[0], (*vec)[1], (*vec)[2]);
+                        ++K;
+                    }
+                }
+                if (K == 0)
+                {
+                    mse_(y, x) = std::numeric_limits<float>::max();
+                    continue;
+                }
+
+                m /= K;
+                m_(y, x) = m;
+
+                // Compute C
+                Matx33f C = Q - K * m * m.t();
+
+                // Compute n
+                SVD svd(C);
+                n_(y, x) = Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2));
+                mse_(y, x) = svd.w.at<float>(2) / K;
+            }
+    }
+
+    /** The size of the block */
+    int block_size_;
+    Mat_<Vec3f> m_;
+    Mat_<Vec3f> n_;
+    Mat_<Vec<float, 9> > Q_;
+    Mat_<float> mse_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class TileQueue
+{
+public:
+    struct PlaneTile
+    {
+        PlaneTile(int x, int y, float mse) :
+            x_(x),
+            y_(y),
+            mse_(mse)
+        { }
+
+        bool operator<(const PlaneTile& tile2) const
+        {
+            return mse_ < tile2.mse_;
+        }
+
+        int x_;
+        int y_;
+        float mse_;
+    };
+
+    TileQueue(const PlaneGrid& plane_grid)
+    {
+        done_tiles_ = Mat_<unsigned char>::zeros(plane_grid.mse_.rows, plane_grid.mse_.cols);
+        tiles_.clear();
+        for (int y = 0; y < plane_grid.mse_.rows; ++y)
+            for (int x = 0; x < plane_grid.mse_.cols; ++x)
+                if (plane_grid.mse_(y, x) != std::numeric_limits<float>::max())
+                    // Update the tiles
+                    tiles_.push_back(PlaneTile(x, y, plane_grid.mse_(y, x)));
+        // Sort tiles by MSE
+        tiles_.sort();
+    }
+
+    bool empty()
+    {
+        while (!tiles_.empty())
+        {
+            const PlaneTile& tile = tiles_.front();
+            if (done_tiles_(tile.y_, tile.x_))
+                tiles_.pop_front();
+            else
+                break;
+        }
+        return tiles_.empty();
+    }
+
+    const PlaneTile& front() const
+    {
+        return tiles_.front();
+    }
+
+    void remove(int y, int x)
+    {
+        done_tiles_(y, x) = 1;
+    }
+private:
+    /** The list of tiles ordered from most planar to least */
+    std::list<PlaneTile> tiles_;
+    /** contains 1 when the tiles has been studied, 0 otherwise */
+    Mat_<unsigned char> done_tiles_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class InlierFinder
+{
+public:
+    InlierFinder(float err, const Mat_<Vec4f>& points3d, const Mat_<Vec4f>& normals,
+                 unsigned char plane_index, int block_size) :
+        err_(err),
+        points3d_(points3d),
+        normals_(normals),
+        plane_index_(plane_index),
+        block_size_(block_size)
+    {
+    }
+
+    void Find(const PlaneGrid& plane_grid, Ptr<PlaneBase>& plane, TileQueue& tile_queue,
+              std::set<TileQueue::PlaneTile>& neighboring_tiles, Mat_<unsigned char>& overall_mask,
+              Mat_<unsigned char>& plane_mask)
+    {
+        // Do not use reference as we pop the from later on
+        TileQueue::PlaneTile tile = *(neighboring_tiles.begin());
+
+        // Figure the part of the image to look at
+        Range range_x, range_y;
+        int x = tile.x_ * block_size_, y = tile.y_ * block_size_;
+
+        if (tile.x_ == plane_mask.cols - 1)
+            range_x = Range(x, overall_mask.cols);
+        else
+            range_x = Range(x, x + block_size_);
+
+        if (tile.y_ == plane_mask.rows - 1)
+            range_y = Range(y, overall_mask.rows);
+        else
+            range_y = Range(y, y + block_size_);
+
+        int n_valid_points = 0;
+        for (int yy = range_y.start; yy != range_y.end; ++yy)
+        {
+            uchar* data = overall_mask.ptr(yy, range_x.start), * data_end = data + range_x.size();
+            const Vec4f* point = points3d_.ptr < Vec4f >(yy, range_x.start);
+            const Matx33f* Q_local = reinterpret_cast<const Matx33f*>(plane_grid.Q_.ptr < Vec<float, 9>
+            >(yy, range_x.start));
+
+            // Depending on whether you have a normal, check it
+            if (!normals_.empty())
+            {
+                const Vec4f* normal = normals_.ptr < Vec4f >(yy, range_x.start);
+                for (; data != data_end; ++data, ++point, ++normal, ++Q_local)
+                {
+                    // Don't do anything if the point already belongs to another plane
+                    if (cvIsNaN(point->val[0]) || ((*data) != 255))
+                        continue;
+
+                    // If the point is close enough to the plane
+                    Vec3f _p = Vec3f((*point)[0], (*point)[1], (*point)[2]);
+                    if (plane->distance(_p) < err_)
+                    {
+                        // make sure the normals are similar to the plane
+                        Vec3f _n = Vec3f((*normal)[0], (*normal)[1], (*normal)[2]);
+                        if (std::abs(plane->n().dot(_n)) > 0.3)
+                        {
+                            // The point now belongs to the plane
+                            plane->UpdateStatistics(_p, *Q_local);
+                            *data = plane_index_;
+                            ++n_valid_points;
+                        }
+                    }
+                }
+            }
+            else
+            {
+                for (; data != data_end; ++data, ++point, ++Q_local)
+                {
+                    // Don't do anything if the point already belongs to another plane
+                    if (cvIsNaN(point->val[0]) || ((*data) != 255))
+                        continue;
+
+                    // If the point is close enough to the plane
+                    Vec3f _p = Vec3f((*point)[0], (*point)[1], (*point)[2]);
+                    if (plane->distance(_p) < err_)
+                    {
+                        // The point now belongs to the plane
+                        plane->UpdateStatistics(_p, *Q_local);
+                        *data = plane_index_;
+                        ++n_valid_points;
+                    }
+                }
+            }
+        }
+
+        plane->UpdateParameters();
+
+        // Mark the front as being done and pop it
+        if (n_valid_points > (range_x.size() * range_y.size()) / 2)
+            tile_queue.remove(tile.y_, tile.x_);
+        plane_mask(tile.y_, tile.x_) = 1;
+        neighboring_tiles.erase(neighboring_tiles.begin());
+
+        // Add potential neighbors of the tile
+        std::vector<std::pair<int, int> > pairs;
+        if (tile.x_ > 0)
+            for (unsigned char* val = overall_mask.ptr<unsigned char>(range_y.start, range_x.start), *val_end = val
+                + range_y.size() * overall_mask.step; val != val_end; val += overall_mask.step)
+                if (*val == plane_index_)
+                {
+                    pairs.push_back(std::pair<int, int>(tile.x_ - 1, tile.y_));
+                    break;
+                }
+        if (tile.x_ < plane_mask.cols - 1)
+            for (unsigned char* val = overall_mask.ptr<unsigned char>(range_y.start, range_x.end - 1), *val_end = val
+                + range_y.size() * overall_mask.step; val != val_end; val += overall_mask.step)
+                if (*val == plane_index_)
+                {
+                    pairs.push_back(std::pair<int, int>(tile.x_ + 1, tile.y_));
+                    break;
+                }
+        if (tile.y_ > 0)
+            for (unsigned char* val = overall_mask.ptr<unsigned char>(range_y.start, range_x.start), *val_end = val
+                + range_x.size(); val != val_end; ++val)
+                if (*val == plane_index_)
+                {
+                    pairs.push_back(std::pair<int, int>(tile.x_, tile.y_ - 1));
+                    break;
+                }
+        if (tile.y_ < plane_mask.rows - 1)
+            for (unsigned char* val = overall_mask.ptr<unsigned char>(range_y.end - 1, range_x.start), *val_end = val
+                + range_x.size(); val != val_end; ++val)
+                if (*val == plane_index_)
+                {
+                    pairs.push_back(std::pair<int, int>(tile.x_, tile.y_ + 1));
+                    break;
+                }
+
+        for (unsigned char i = 0; i < pairs.size(); ++i)
+            if (!plane_mask(pairs[i].second, pairs[i].first))
+                neighboring_tiles.insert(
+                    TileQueue::PlaneTile(pairs[i].first, pairs[i].second, plane_grid.mse_(pairs[i].second, pairs[i].first)));
+    }
+
+private:
+    float err_;
+    const Mat_<Vec4f>& points3d_;
+    const Mat_<Vec4f>& normals_;
+    unsigned char plane_index_;
+    /** THe block size as defined in the main algorithm */
+    int block_size_;
+
+    const InlierFinder& operator = (const InlierFinder&);
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void findPlanes(InputArray points3d_in, InputArray normals_in, OutputArray mask_out, OutputArray plane_coefficients_out,
+                int block_size, int min_size, double threshold, double sensor_error_a, double sensor_error_b, double sensor_error_c,
+                RgbdPlaneMethod method)
+{
+    CV_Assert(method == RGBD_PLANE_METHOD_DEFAULT);
+
+    Mat_<Vec4f> points3d, normals;
+    if (points3d_in.depth() == CV_32F)
+        points3d = points3d_in.getMat();
+    else
+        points3d_in.getMat().convertTo(points3d, CV_32F);
+    if (!normals_in.empty())
+    {
+        if (normals_in.depth() == CV_32F)
+            normals = normals_in.getMat();
+        else
+            normals_in.getMat().convertTo(normals, CV_32F);
+    }
+
+    // Pre-computations
+    mask_out.create(points3d.size(), CV_8U);
+    Mat mask_out_mat = mask_out.getMat();
+    Mat_<unsigned char> mask_out_uc = (Mat_<unsigned char>&) mask_out_mat;
+    mask_out_uc.setTo(255);
+    PlaneGrid plane_grid(points3d, block_size);
+    TileQueue plane_queue(plane_grid);
+    size_t index_plane = 0;
+
+    std::vector<Vec4f> plane_coefficients;
+    float mse_min = (float)(threshold * threshold);
+
+    while (!plane_queue.empty())
+    {
+        // Get the first tile if it's good enough
+        const TileQueue::PlaneTile front_tile = plane_queue.front();
+        if (front_tile.mse_ > mse_min)
+            break;
+
+        InlierFinder inlier_finder((float)threshold, points3d, normals, (unsigned char)index_plane, block_size);
+
+        // Construct the plane for the first tile
+        int x = front_tile.x_, y = front_tile.y_;
+        const Vec3f& n = plane_grid.n_(y, x);
+        Ptr<PlaneBase> plane;
+        if ((sensor_error_a == 0) && (sensor_error_b == 0) && (sensor_error_c == 0))
+            plane = Ptr<PlaneBase>(new Plane(plane_grid.m_(y, x), n, (int)index_plane));
+        else
+            plane = Ptr<PlaneBase>(new PlaneABC(plane_grid.m_(y, x), n, (int)index_plane,
+                                   (float)sensor_error_a, (float)sensor_error_b, (float)sensor_error_c));
+
+        Mat_<unsigned char> plane_mask = Mat_<unsigned char>::zeros(divUp(points3d.rows, block_size),
+                                                                    divUp(points3d.cols, block_size));
+        std::set<TileQueue::PlaneTile> neighboring_tiles;
+        neighboring_tiles.insert(front_tile);
+        plane_queue.remove(front_tile.y_, front_tile.x_);
+
+        // Process all the neighboring tiles
+        while (!neighboring_tiles.empty())
+            inlier_finder.Find(plane_grid, plane, plane_queue, neighboring_tiles, mask_out_uc, plane_mask);
+
+        // Don't record the plane if it's empty
+        if (plane->empty())
+            continue;
+        // Don't record the plane if it's smaller than asked
+        if (plane->K() < min_size)
+        {
+            // Reset the plane index in the mask
+            for (y = 0; y < plane_mask.rows; ++y)
+                for (x = 0; x < plane_mask.cols; ++x)
+                {
+                    if (!plane_mask(y, x))
+                        continue;
+                    // Go over the tile
+                    for (int yy = y * block_size;
+                        yy < std::min((y + 1) * block_size, mask_out_uc.rows); ++yy)
+                    {
+                        uchar* data = mask_out_uc.ptr(yy, x * block_size);
+                        uchar* data_end = data + std::min(block_size, mask_out_uc.cols - x * block_size);
+                        for (; data != data_end; ++data)
+                        {
+                            if (*data == index_plane)
+                                *data = 255;
+                        }
+                    }
+                }
+            continue;
+        }
+
+        ++index_plane;
+        if (index_plane >= 255)
+            break;
+        Vec4f coeffs(plane->n()[0], plane->n()[1], plane->n()[2], plane->d());
+        if (coeffs(2) > 0)
+            coeffs = -coeffs;
+        plane_coefficients.push_back(coeffs);
+    };
+
+    // Fill the plane coefficients
+    if (plane_coefficients.empty())
+        return;
+    plane_coefficients_out.create((int)plane_coefficients.size(), 1, CV_32FC4);
+    Mat plane_coefficients_mat = plane_coefficients_out.getMat();
+    float* data = plane_coefficients_mat.ptr<float>(0);
+    for (size_t i = 0; i < plane_coefficients.size(); ++i)
+        for (uchar j = 0; j < 4; ++j, ++data)
+            *data = plane_coefficients[i][j];
+}
+
+} // namespace cv

modules/ptcloud/src/precomp.hpp

@@ -0,0 +1,28 @@
+#ifndef __OPENCV_PRECOMP_H__
+#define __OPENCV_PRECOMP_H__
+
+#ifdef _MSC_VER
+#  define _SILENCE_CXX17_C_HEADER_DEPRECATION_WARNING
+#  define _SILENCE_CXX17_CODECVT_HEADER_DEPRECATION_WARNING
+#endif
+
+#include <vector>
+#include <set>
+#include <map>
+#include <list>
+#include <unordered_set>
+#include <unordered_map>
+#include <stack>
+
+#include "opencv2/core.hpp"
+#include "opencv2/core/types.hpp"
+#include "opencv2/core/base.hpp"
+#include "opencv2/core/utils/logger.hpp"
+#include "opencv2/core/utils/trace.hpp"
+#include "opencv2/core/ocl.hpp"
+#include "opencv2/core/hal/intrin.hpp"
+#include "opencv2/ptcloud.hpp"
+#include "opencv2/geometry.hpp"
+#include "opencv2/imgproc.hpp"
+
+#endif

modules/ptcloud/src/rendering.cpp

@@ -0,0 +1,412 @@
+// 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
+
+#include "precomp.hpp"
+
+namespace cv {
+
+TriangleRasterizeSettings::TriangleRasterizeSettings()
+{
+    shadingType = RASTERIZE_SHADING_SHADED;
+    cullingMode = RASTERIZE_CULLING_CW;
+    glCompatibleMode = RASTERIZE_COMPAT_DISABLED;
+}
+
+static void drawTriangle(Vec4f verts[3], Vec3f colors[3], Mat& depthBuf, Mat& colorBuf,
+                         TriangleRasterizeSettings settings)
+{
+    // this will be useful during refactoring
+    // if there's gonna be more supported data types
+    CV_DbgAssert(depthBuf.empty() || depthBuf.type() == CV_32FC1);
+    CV_DbgAssert(colorBuf.empty() || colorBuf.type() == CV_32FC3);
+
+    // any of buffers can be empty
+    int width  = std::max(colorBuf.cols, depthBuf.cols);
+    int height = std::max(colorBuf.rows, depthBuf.rows);
+
+    Point minPt(width, height), maxPt(0, 0);
+    for (int i = 0; i < 3; i++)
+    {
+        // round down to cover the whole pixel
+        int x = (int)(verts[i][0]), y = (int)(verts[i][1]);
+        minPt.x = std::min(    x, minPt.x);
+        minPt.y = std::min(    y, minPt.y);
+        maxPt.x = std::max(x + 1, maxPt.x);
+        maxPt.y = std::max(y + 1, maxPt.y);
+    }
+
+    minPt.x = std::max(minPt.x, 0); maxPt.x = std::min(maxPt.x, width);
+    minPt.y = std::max(minPt.y, 0); maxPt.y = std::min(maxPt.y, height);
+
+    Point2f a(verts[0][0], verts[0][1]), b(verts[1][0], verts[1][1]), c(verts[2][0], verts[2][1]);
+    Point2f bc = b - c, ac = a - c;
+    float d = ac.x*bc.y - ac.y*bc.x;
+
+    // culling and degenerated triangle removal
+    if ((settings.cullingMode == RASTERIZE_CULLING_CW  && d <= 0) ||
+        (settings.cullingMode == RASTERIZE_CULLING_CCW && d >= 0) ||
+        (abs(d) < 1e-6))
+    {
+        return;
+    }
+
+    float invd = 1.f / d;
+    Vec3f zinv { verts[0][2], verts[1][2], verts[2][2] };
+    Vec3f w { verts[0][3], verts[1][3], verts[2][3] };
+
+    for (int y = minPt.y; y < maxPt.y; y++)
+    {
+        for (int x = minPt.x; x < maxPt.x; x++)
+        {
+            Point2f p(x + 0.5f, y + 0.5f), pc = p - c;
+            // barycentric coordinates
+            Vec3f f;
+            f[0] = ( pc.x * bc.y - pc.y * bc.x) * invd;
+            f[1] = ( pc.y * ac.x - pc.x * ac.y) * invd;
+            f[2] = 1.f - f[0] - f[1];
+            // if inside the triangle
+            if ((f[0] >= 0) && (f[1] >= 0) && (f[2] >= 0))
+            {
+                bool update = false;
+                if (!depthBuf.empty())
+                {
+                    float zCurrent = depthBuf.at<float>(height - 1 - y, x);
+                    float zNew = f[0] * zinv[0] + f[1] * zinv[1] + f[2] * zinv[2];
+                    if (zNew < zCurrent)
+                    {
+                        update = true;
+                        depthBuf.at<float>(height - 1 - y, x) = zNew;
+                    }
+                }
+                else // RASTERIZE_SHADING_WHITE
+                {
+                    update = true;
+                }
+
+                if (!colorBuf.empty() && update)
+                {
+                    Vec3f color;
+                    if (settings.shadingType == RASTERIZE_SHADING_WHITE)
+                    {
+                        color = { 1.f, 1.f, 1.f };
+                    }
+                    else if (settings.shadingType == RASTERIZE_SHADING_FLAT)
+                    {
+                        color = colors[0];
+                    }
+                    else // TriangleShadingType::Shaded
+                    {
+                        float zInter = 1.0f / (f[0] * w[0] + f[1] * w[1] + f[2] * w[2]);
+                        color = { 0, 0, 0 };
+                        for (int j = 0; j < 3; j++)
+                        {
+                            color += (f[j] * w[j]) * colors[j];
+                        }
+                        color *= zInter;
+                    }
+                    colorBuf.at<Vec3f>(height - 1 - y, x) = color;
+                }
+            }
+        }
+    }
+}
+
+
+// values outside of [zNear, zFar] have to be restored
+// [0, 1] -> [zNear, zFar]
+static void linearizeDepth(const Mat& inbuf, const Mat& validMask, Mat outbuf, double zFar, double zNear)
+{
+    CV_Assert(inbuf.type() == CV_32FC1);
+    CV_Assert(validMask.type() == CV_8UC1 || validMask.type() == CV_8SC1 || validMask.type() == CV_BoolC1);
+    CV_Assert(outbuf.type() == CV_32FC1);
+    CV_Assert(outbuf.size() == inbuf.size());
+
+    float scaleNear = (float)(1.0 / zNear);
+    float scaleFar  = (float)(1.0 / zFar);
+    for (int y = 0; y < inbuf.rows; y++)
+    {
+        const float* inp = inbuf.ptr<float>(y);
+        const uchar * validPtr = validMask.ptr<uchar>(y);
+        float * outp = outbuf.ptr<float>(y);
+        for (int x = 0; x < inbuf.cols; x++)
+        {
+            if (validPtr[x])
+            {
+                float d = inp[x];
+                // precision-optimized version of this:
+                //float z = - zFar * zNear / (d * (zFar - zNear) - zFar);
+                float z =  1.f / ((1.f - d) * scaleNear + d * scaleFar );
+                outp[x] = z;
+            }
+        }
+    }
+}
+
+// [zNear, zFar] -> [0, 1]
+static void invertDepth(const Mat& inbuf, Mat& outbuf, Mat& validMask, double zNear, double zFar)
+{
+    CV_Assert(inbuf.type() == CV_32FC1);
+    outbuf.create(inbuf.size(), CV_32FC1);
+    validMask.create(inbuf.size(), CV_8UC1);
+
+    float fNear = (float)zNear, fFar = (float)zFar;
+    float zadd = (float)(zFar / (zFar - zNear));
+    float zmul = (float)(-zNear * zFar / (zFar - zNear));
+    for (int y = 0; y < inbuf.rows; y++)
+    {
+        const float * inp = inbuf.ptr<float>(y);
+        float * outp = outbuf.ptr<float>(y);
+        uchar * validPtr = validMask.ptr<uchar>(y);
+        for (int x = 0; x < inbuf.cols; x++)
+        {
+            float z = inp[x];
+            uchar m = (z >= fNear) && (z <= fFar);
+            z = std::max(std::min(z, fFar), fNear);
+            // precision-optimized version of this:
+            // outp[x] = (z - zNear) / z * zFar / (zFar - zNear);
+            outp[x] = zadd + zmul / z;
+            validPtr[x] = m;
+        }
+    }
+}
+
+
+static void triangleRasterizeInternal(InputArray _vertices, InputArray _indices, InputArray _colors,
+                                      Mat& colorBuf, Mat& depthBuf,
+                                      InputArray world2cam, double fovyRadians, double zNear, double zFar,
+                                      const TriangleRasterizeSettings& settings)
+{
+    CV_Assert(world2cam.type() == CV_32FC1 || world2cam.type() == CV_64FC1);
+    CV_Assert((world2cam.size() == Size {4, 3}) || (world2cam.size() == Size {4, 4}));
+
+    CV_Assert((fovyRadians > 0) && (fovyRadians < CV_PI));
+    CV_Assert(zNear > 0);
+    CV_Assert(zFar > zNear);
+
+    Mat cpMat;
+    world2cam.getMat().convertTo(cpMat, CV_64FC1);
+    Matx44d camPoseMat = Matx44d::eye();
+    for (int i = 0; i < 3; i++)
+    {
+        for (int j = 0; j < 4; j++)
+        {
+            camPoseMat(i, j) = cpMat.at<double>(i, j);
+        }
+    }
+
+    if(_indices.empty())
+    {
+        return;
+    }
+
+    CV_CheckFalse(_vertices.empty(), "No vertices provided along with indices array");
+
+    Mat vertices, colors, triangles;
+    int nVerts = 0, nColors = 0, nTriangles = 0;
+
+    int vertexType = _vertices.type();
+    CV_Assert(vertexType == CV_32FC1 || vertexType == CV_32FC3);
+    vertices = _vertices.getMat();
+    // transform 3xN matrix to Nx3, except 3x3
+    if ((_vertices.channels() == 1) && (_vertices.rows() == 3) && (_vertices.cols() != 3))
+    {
+        vertices = vertices.t();
+    }
+    // This transposition is performed on 1xN matrix so it's almost free in terms of performance
+    vertices = vertices.reshape(3, 1).t();
+    nVerts = (int)vertices.total();
+
+    int indexType = _indices.type();
+    CV_Assert(indexType == CV_32SC1 || indexType == CV_32SC3);
+    triangles = _indices.getMat();
+    // transform 3xN matrix to Nx3, except 3x3
+    if ((_indices.channels() == 1) && (_indices.rows() == 3) && (_indices.cols() != 3))
+    {
+        triangles = triangles.t();
+    }
+    // This transposition is performed on 1xN matrix so it's almost free in terms of performance
+    triangles = triangles.reshape(3, 1).t();
+    nTriangles = (int)triangles.total();
+
+    if (!_colors.empty())
+    {
+        int colorType = _colors.type();
+        CV_Assert(colorType == CV_32FC1 || colorType == CV_32FC3);
+        colors = _colors.getMat();
+        // transform 3xN matrix to Nx3, except 3x3
+        if ((_colors.channels() == 1) && (_colors.rows() == 3) && (_colors.cols() != 3))
+        {
+            colors = colors.t();
+        }
+        colors = colors.reshape(3, 1).t();
+        nColors = (int)colors.total();
+
+        CV_Assert(nColors == nVerts);
+    }
+
+    // any of buffers can be empty
+    Size imgSize {std::max(colorBuf.cols, depthBuf.cols), std::max(colorBuf.rows, depthBuf.rows)};
+
+    // world-to-camera coord system
+    Matx44d lookAtMatrix = camPoseMat;
+
+    double ys = 1.0 / std::tan(fovyRadians / 2);
+    double xs = ys / (double)imgSize.width * (double)imgSize.height;
+    double zz = (zNear + zFar) / (zNear - zFar);
+    double zw = 2.0 * zFar * zNear / (zNear - zFar);
+
+    // camera to NDC: [-1, 1]^3
+    Matx44d perspectMatrix (xs,  0,  0,  0,
+                             0, ys,  0,  0,
+                             0,  0, zz, zw,
+                             0,  0,  -1, 0);
+
+    Matx44f mvpMatrix = perspectMatrix * lookAtMatrix;
+
+    // vertex transform stage
+
+    Mat screenVertices(vertices.size(), CV_32FC4);
+    for (int i = 0; i < nVerts; i++)
+    {
+        Vec3f vglobal = vertices.at<Vec3f>(i);
+
+        float x_num = std::fma(mvpMatrix(0,0), vglobal[0],
+                      std::fma(mvpMatrix(0,1), vglobal[1],
+                      std::fma(mvpMatrix(0,2), vglobal[2], mvpMatrix(0,3))));
+        float y_num = std::fma(mvpMatrix(1,0), vglobal[0],
+                      std::fma(mvpMatrix(1,1), vglobal[1],
+                      std::fma(mvpMatrix(1,2), vglobal[2], mvpMatrix(1,3))));
+        float z_num = std::fma(mvpMatrix(2,0), vglobal[0],
+                      std::fma(mvpMatrix(2,1), vglobal[1],
+                      std::fma(mvpMatrix(2,2), vglobal[2], mvpMatrix(2,3))));
+        float w_num = std::fma(mvpMatrix(3,0), vglobal[0],
+                      std::fma(mvpMatrix(3,1), vglobal[1],
+                      std::fma(mvpMatrix(3,2), vglobal[2], mvpMatrix(3,3))));
+
+        float invw = 1.f / w_num;
+
+        // [-1, 1]^3 => [0, width] x [0, height] x [0, 1]
+        Vec4f vscreen = {
+            std::fma(x_num * invw, 0.5f * (float)imgSize.width,  0.5f * (float)imgSize.width),
+            std::fma(y_num * invw, 0.5f * (float)imgSize.height, 0.5f * (float)imgSize.height),
+            std::fma(z_num * invw, 0.5f, 0.5f),
+            invw
+        };
+
+        screenVertices.at<Vec4f>(i) = vscreen;
+    }
+
+    // draw stage
+
+    for (int t = 0; t < nTriangles; t++)
+    {
+        Vec3i tri = triangles.at<Vec3i>(t);
+
+        Vec3f col[3];
+        Vec4f ver[3];
+        for (int i = 0; i < 3; i++)
+        {
+            int idx = tri[i];
+            CV_DbgAssert(idx >= 0 && idx < nVerts);
+
+            col[i] = colors.empty() ? Vec3f::all(0) : colors.at<Vec3f>(idx);
+            ver[i] = screenVertices.at<Vec4f>(idx);
+        }
+
+        drawTriangle(ver, col, depthBuf, colorBuf, settings);
+    }
+}
+
+
+void triangleRasterizeDepth(InputArray _vertices, InputArray _indices, InputOutputArray _depthBuf,
+                            InputArray world2cam, double fovY, double zNear, double zFar,
+                            const TriangleRasterizeSettings& settings)
+{
+    CV_Assert(!_depthBuf.empty());
+    CV_Assert(_depthBuf.type() == CV_32FC1);
+
+    Mat emptyColorBuf;
+    // out-of-range values from user-provided depthBuf should not be altered, let's mark them
+    Mat_<uchar> validMask;
+    Mat depthBuf;
+    if (settings.glCompatibleMode == RASTERIZE_COMPAT_INVDEPTH)
+    {
+        depthBuf = _depthBuf.getMat();
+    }
+    else // RASTERIZE_COMPAT_DISABLED
+    {
+        invertDepth(_depthBuf.getMat(), depthBuf, validMask, zNear, zFar);
+    }
+
+    triangleRasterizeInternal(_vertices, _indices, noArray(), emptyColorBuf, depthBuf, world2cam, fovY, zNear, zFar, settings);
+
+    if (settings.glCompatibleMode == RASTERIZE_COMPAT_DISABLED)
+    {
+        linearizeDepth(depthBuf, validMask, _depthBuf.getMat(), zFar, zNear);
+    }
+}
+
+void triangleRasterizeColor(InputArray _vertices, InputArray _indices, InputArray _colors, InputOutputArray _colorBuf,
+                            InputArray world2cam, double fovY, double zNear, double zFar,
+                            const TriangleRasterizeSettings& settings)
+{
+    CV_Assert(!_colorBuf.empty());
+    CV_Assert(_colorBuf.type() == CV_32FC3);
+    Mat colorBuf = _colorBuf.getMat();
+
+    Mat depthBuf;
+    if (_colors.empty())
+    {
+        // full white shading does not require depth test
+        CV_Assert(settings.shadingType == RASTERIZE_SHADING_WHITE);
+    }
+    else
+    {
+        // internal depth buffer is not exposed outside
+        depthBuf.create(_colorBuf.size(), CV_32FC1);
+        depthBuf.setTo(1.0);
+    }
+
+    triangleRasterizeInternal(_vertices, _indices, _colors, colorBuf, depthBuf, world2cam, fovY, zNear, zFar, settings);
+}
+
+void triangleRasterize(InputArray _vertices, InputArray _indices, InputArray _colors,
+                       InputOutputArray _colorBuffer, InputOutputArray _depthBuffer,
+                       InputArray world2cam, double fovyRadians, double zNear, double zFar,
+                       const TriangleRasterizeSettings& settings)
+{
+    if (_colors.empty())
+    {
+        CV_Assert(settings.shadingType == RASTERIZE_SHADING_WHITE);
+    }
+
+    CV_Assert(!_colorBuffer.empty());
+    CV_Assert(_colorBuffer.type() == CV_32FC3);
+    CV_Assert(!_depthBuffer.empty());
+    CV_Assert(_depthBuffer.type() == CV_32FC1);
+
+    CV_Assert(_depthBuffer.size() == _colorBuffer.size());
+
+    Mat colorBuf = _colorBuffer.getMat();
+
+    // out-of-range values from user-provided depthBuf should not be altered, let's mark them
+    Mat_<uchar> validMask;
+    Mat depthBuf;
+    if (settings.glCompatibleMode == RASTERIZE_COMPAT_INVDEPTH)
+    {
+        depthBuf = _depthBuffer.getMat();
+    }
+    else // RASTERIZE_COMPAT_DISABLED
+    {
+        invertDepth(_depthBuffer.getMat(), depthBuf, validMask, zNear, zFar);
+    }
+
+    triangleRasterizeInternal(_vertices, _indices, _colors, colorBuf, depthBuf, world2cam, fovyRadians, zNear, zFar, settings);
+
+    if (settings.glCompatibleMode == RASTERIZE_COMPAT_DISABLED)
+    {
+        linearizeDepth(depthBuf, validMask, _depthBuffer.getMat(), zFar, zNear);
+    }
+}
+} // namespace cv

modules/ptcloud/src/sparse_block_matrix.hpp

@@ -0,0 +1,223 @@
+// 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_SPARSE_BLOCK_MATRIX_HPP
+#define OPENCV_3D_SPARSE_BLOCK_MATRIX_HPP
+
+#include "precomp.hpp"
+
+#if defined(HAVE_EIGEN)
+#include <Eigen/Core>
+#include <Eigen/Sparse>
+#include <Eigen/SparseCholesky>
+
+#include "opencv2/core/eigen.hpp"
+#endif
+
+namespace cv
+{
+/*!
+ * \class BlockSparseMat
+ * Naive implementation of Sparse Block Matrix
+ */
+template<typename _Tp, size_t blockM, size_t blockN>
+struct BlockSparseMat
+{
+    struct Point2iHash
+    {
+        size_t operator()(const cv::Point2i& point) const noexcept
+        {
+            size_t seed                     = 0;
+            constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
+            seed ^= std::hash<int>()(point.x) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+            seed ^= std::hash<int>()(point.y) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
+            return seed;
+        }
+    };
+    typedef Matx<_Tp, blockM, blockN> MatType;
+    typedef std::unordered_map<Point2i, MatType, Point2iHash> IDtoBlockValueMap;
+
+    BlockSparseMat(size_t _nBlocks) : nBlocks(_nBlocks), ijValue() {}
+
+    void clear()
+    {
+        ijValue.clear();
+    }
+
+    inline MatType& refBlock(size_t i, size_t j)
+    {
+        Point2i p((int)i, (int)j);
+        auto it = ijValue.find(p);
+        if (it == ijValue.end())
+        {
+            it = ijValue.insert({ p, MatType::zeros() }).first;
+        }
+        return it->second;
+    }
+
+    inline _Tp& refElem(size_t i, size_t j)
+    {
+        Point2i ib((int)(i / blockM), (int)(j / blockN));
+        Point2i iv((int)(i % blockM), (int)(j % blockN));
+        return refBlock(ib.x, ib.y)(iv.x, iv.y);
+    }
+
+    inline MatType valBlock(size_t i, size_t j) const
+    {
+        Point2i p((int)i, (int)j);
+        auto it = ijValue.find(p);
+        if (it == ijValue.end())
+            return MatType::zeros();
+        else
+            return it->second;
+    }
+
+    inline _Tp valElem(size_t i, size_t j) const
+    {
+        Point2i ib((int)(i / blockM), (int)(j / blockN));
+        Point2i iv((int)(i % blockM), (int)(j % blockN));
+        return valBlock(ib.x, ib.y)(iv.x, iv.y);
+    }
+
+    Mat diagonal() const
+    {
+        // Diagonal max length is the number of columns in the sparse matrix
+        int diagLength =int( blockN * nBlocks );
+        cv::Mat diag   = cv::Mat::zeros(diagLength, 1, cv::DataType<_Tp>::type);
+
+        for (int i = 0; i < diagLength; i++)
+        {
+            diag.at<_Tp>(i, 0) = valElem(i, i);
+        }
+        return diag;
+    }
+
+#if defined(HAVE_EIGEN)
+    Eigen::SparseMatrix<_Tp> toEigen() const
+    {
+        std::vector<Eigen::Triplet<_Tp>> tripletList;
+        tripletList.reserve(ijValue.size() * blockM * blockN);
+        for (const auto& ijv : ijValue)
+        {
+            int xb = ijv.first.x, yb = ijv.first.y;
+            MatType vblock = ijv.second;
+            for (size_t i = 0; i < blockM; i++)
+            {
+                for (size_t j = 0; j < blockN; j++)
+                {
+                    _Tp val = vblock((int)i, (int)j);
+                    if (abs(val) >= NON_ZERO_VAL_THRESHOLD)
+                    {
+                        tripletList.push_back(Eigen::Triplet<_Tp>((int)(blockM * xb + i), (int)(blockN * yb + j), val));
+                    }
+                }
+            }
+        }
+        Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * nBlocks);
+        EigenMat.setFromTriplets(tripletList.begin(), tripletList.end());
+        EigenMat.makeCompressed();
+
+        return EigenMat;
+    }
+#endif
+    inline size_t nonZeroBlocks() const { return ijValue.size(); }
+
+    BlockSparseMat<_Tp, blockM, blockN>& operator+=(const BlockSparseMat<_Tp, blockM, blockN>& other)
+    {
+        for (const auto& oijv : other.ijValue)
+        {
+            Point2i p = oijv.first;
+            MatType vblock = oijv.second;
+            this->refBlock(p.x, p.y) += vblock;
+        }
+
+        return *this;
+    }
+
+#if defined(HAVE_EIGEN)
+
+    // Decomposes matrix for further solution
+    // Sometimes it's required to consequently solve A*x = b then A*x = c then A*x = d...
+    // Splitting the solution procedure into two parts let us reuse the matrix' decomposition
+    struct Decomposition
+    {
+        Eigen::SparseMatrix<_Tp> bigA;
+        Eigen::SimplicialLDLT<Eigen::SparseMatrix<_Tp>> solver;
+    };
+
+    bool decompose(Decomposition& d, bool checkSymmetry = true) const
+    {
+        d.bigA = this->toEigen();
+
+        Eigen::SparseMatrix<_Tp> bigAtranspose = d.bigA.transpose();
+        if (checkSymmetry && !d.bigA.isApprox(bigAtranspose))
+        {
+            CV_Error(Error::StsBadArg, "H matrix is not symmetrical");
+            return false;
+        }
+
+        d.solver.compute(d.bigA);
+        bool r = (d.solver.info() == Eigen::Success);
+        if (!r)
+        {
+            CV_LOG_INFO(NULL, "Failed to eigen-decompose");
+        }
+
+        return r;
+    }
+
+    static bool solveDecomposed(const Decomposition& d, InputArray B, OutputArray X, OutputArray predB = cv::noArray())
+    {
+        Mat mb = B.getMat();
+        mb = mb.cols == 1 ? mb : mb.t();
+        Eigen::Matrix<_Tp, -1, 1> bigB;
+        cv2eigen(mb, bigB);
+
+        Eigen::Matrix<_Tp, -1, 1> solutionX = d.solver.solve(bigB);
+        if (d.solver.info() != Eigen::Success)
+        {
+            CV_LOG_INFO(NULL, "Failed to eigen-solve");
+            return false;
+        }
+        else
+        {
+            eigen2cv(solutionX, X);
+            if (predB.needed())
+            {
+                Eigen::Matrix<_Tp, -1, 1> predBEigen = d.bigA * solutionX;
+                eigen2cv(predBEigen, predB);
+            }
+            return true;
+        }
+    }
+
+#else
+    struct Decomposition { };
+
+    bool decompose(Decomposition& /*_d*/, bool /*checkSymmetry*/ = true) const
+    {
+        CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented");
+    }
+
+    bool solveDecomposed(const Decomposition& /*d*/, InputArray /*B*/, OutputArray /*X*/, OutputArray /*predB*/ = cv::noArray()) const
+    {
+        CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented");
+    }
+#endif
+
+    //! Function to solve a sparse linear system of equations HX = B
+    bool sparseSolve(InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) const
+    {
+        Decomposition d;
+        return decompose(d, checkSymmetry) && solveDecomposed(d, B, X, predB);
+    }
+
+    static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001);
+    size_t nBlocks;
+    IDtoBlockValueMap ijValue;
+};
+
+}  // namespace cv
+
+#endif // include guard

modules/ptcloud/src/tsdf_functions.hpp

@@ -0,0 +1,211 @@
+// 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
+
+// Partially rewritten from https://github.com/Nerei/kinfu_remake
+// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
+
+#ifndef OPENCV_3D_TSDF_FUNCTIONS_HPP
+#define OPENCV_3D_TSDF_FUNCTIONS_HPP
+
+#include "precomp.hpp"
+#include "utils.hpp"
+
+namespace cv
+{
+
+typedef int8_t TsdfType;
+typedef uchar WeightType;
+
+struct TsdfVoxel
+{
+    TsdfVoxel(TsdfType _tsdf, WeightType _weight) :
+        tsdf(_tsdf), weight(_weight)
+    { }
+    TsdfType tsdf;
+    WeightType weight;
+};
+
+typedef Vec<uchar, sizeof(TsdfVoxel)> VecTsdfVoxel;
+
+typedef short int ColorType;
+struct RGBTsdfVoxel
+{
+    RGBTsdfVoxel(TsdfType _tsdf, WeightType _weight, ColorType _r, ColorType _g, ColorType _b) :
+        tsdf(_tsdf), weight(_weight), r(_r), g(_g), b(_b)
+    { }
+    TsdfType tsdf;
+    WeightType weight;
+    ColorType r, g, b;
+};
+
+typedef Vec<uchar, sizeof(RGBTsdfVoxel)> VecRGBTsdfVoxel;
+
+#if CV_SIMD128
+inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num)
+{
+    v_float32x4 num128 = v_setall_f32(-1.f / 128.f);
+    return v_mul(v_cvt_f32(num), num128);
+}
+#endif
+
+inline TsdfType floatToTsdf(float num)
+{
+    //CV_Assert(-1 < num <= 1);
+    int8_t res = int8_t(num * (-128.f));
+    res = res ? res : (num < 0 ? 1 : -1);
+    return res;
+}
+
+inline float tsdfToFloat(TsdfType num)
+{
+    return float(num) * (-1.f / 128.f);
+}
+
+inline void colorFix(ColorType& r, ColorType& g, ColorType&b)
+{
+    if (r > 255) r = 255;
+    if (g > 255) g = 255;
+    if (b > 255) b = 255;
+}
+
+inline void colorFix(Point3f& c)
+{
+    if (c.x > 255) c.x = 255;
+    if (c.y > 255) c.y = 255;
+    if (c.z > 255) c.z = 255;
+}
+
+void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm);
+#ifdef HAVE_OPENCL
+void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm);
+#endif
+
+inline depthType bilinearDepth(const Depth& m, cv::Point2f pt)
+{
+    const bool fixMissingData = false;
+    const depthType defaultValue = qnan;
+    if (pt.x < 0 || pt.x >= m.cols - 1 ||
+        pt.y < 0 || pt.y >= m.rows - 1)
+        return defaultValue;
+
+    int xi = cvFloor(pt.x), yi = cvFloor(pt.y);
+
+    const depthType* row0 = m[yi + 0];
+    const depthType* row1 = m[yi + 1];
+
+    depthType v00 = row0[xi + 0];
+    depthType v01 = row0[xi + 1];
+    depthType v10 = row1[xi + 0];
+    depthType v11 = row1[xi + 1];
+
+    // assume correct depth is positive
+    bool b00 = v00 > 0;
+    bool b01 = v01 > 0;
+    bool b10 = v10 > 0;
+    bool b11 = v11 > 0;
+
+    if (!fixMissingData)
+    {
+        if (!(b00 && b01 && b10 && b11))
+            return defaultValue;
+        else
+        {
+            float tx = pt.x - xi, ty = pt.y - yi;
+            depthType v0 = v00 + tx * (v01 - v00);
+            depthType v1 = v10 + tx * (v11 - v10);
+            return v0 + ty * (v1 - v0);
+        }
+    }
+    else
+    {
+        int nz = b00 + b01 + b10 + b11;
+        if (nz == 0)
+        {
+            return defaultValue;
+        }
+        if (nz == 1)
+        {
+            if (b00) return v00;
+            if (b01) return v01;
+            if (b10) return v10;
+            if (b11) return v11;
+        }
+        if (nz == 2)
+        {
+            if (b00 && b10) v01 = v00, v11 = v10;
+            if (b01 && b11) v00 = v01, v10 = v11;
+            if (b00 && b01) v10 = v00, v11 = v01;
+            if (b10 && b11) v00 = v10, v01 = v11;
+            if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f;
+            if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f;
+        }
+        if (nz == 3)
+        {
+            if (!b00) v00 = v10 + v01 - v11;
+            if (!b01) v01 = v00 + v11 - v10;
+            if (!b10) v10 = v00 + v11 - v01;
+            if (!b11) v11 = v01 + v10 - v00;
+        }
+
+        float tx = pt.x - xi, ty = pt.y - yi;
+        depthType v0 = v00 + tx * (v01 - v00);
+        depthType v1 = v10 + tx * (v11 - v10);
+        return v0 + ty * (v1 - v0);
+    }
+}
+
+void _integrateVolumeUnit(
+    float truncDist, float voxelSize, int maxWeight,
+    cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides,
+    InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose,
+    const cv::Intr& intrinsics, InputArray _pixNorms, InputArray _volume);
+
+void _integrateRGBVolumeUnit(
+    float truncDist, float voxelSize, int maxWeight,
+    cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides,
+    InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose,
+    const cv::Intr& depth_intrinsics, const cv::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume);
+
+
+void integrateTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose,
+    InputArray _depth, InputArray _pixNorms, InputArray _volume);
+
+void integrateTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
+    InputArray _depth, InputArray _pixNorms, InputArray _volume);
+
+
+void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr,
+                       InputArray _volume, OutputArray _points, OutputArray _normals);
+
+void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
+                                    InputArray _points, OutputArray _normals);
+
+void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
+                                          OutputArray points, OutputArray normals);
+
+
+#ifdef HAVE_OPENCL
+void ocl_integrateTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose,
+    InputArray _depth, InputArray _pixNorms, InputArray _volume);
+
+void ocl_raycastTsdfVolumeUnit(
+    const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr,
+    InputArray _volume, OutputArray _points, OutputArray _normals);
+
+void ocl_fetchNormalsFromTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volume,
+    InputArray _points, OutputArray _normals);
+
+void ocl_fetchPointsNormalsFromTsdfVolumeUnit(
+    const VolumeSettings& settings, InputArray _volume,
+    OutputArray _points, OutputArray _normals);
+#endif
+
+
+}  // namespace cv
+
+#endif // include guard

modules/ptcloud/src/utils.cpp

@@ -0,0 +1,21 @@
+// 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.
+
+#include "utils.hpp"
+
+namespace cv {
+
+std::vector<std::string> split(const std::string &s, char delimiter)
+{
+    std::vector<std::string> tokens;
+    std::string token;
+    std::istringstream tokenStream(s);
+    while (std::getline(tokenStream, token, delimiter))
+    {
+        tokens.push_back(token);
+    }
+    return tokens;
+}
+
+} /* namespace cv */

modules/ptcloud/src/utils.hpp

@@ -0,0 +1,319 @@
+// 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 _CODERS_UTILS_H_
+#define _CODERS_UTILS_H_
+
+#include "precomp.hpp"
+
+#include <string>
+#include <vector>
+#include <sstream>
+#include <array>
+#include <algorithm>
+#include <cstdint>
+
+namespace cv {
+
+std::vector<std::string> split(const std::string &s, char delimiter);
+
+inline bool startsWith(const std::string &s1, const std::string &s2)
+{
+    return s1.compare(0, s2.length(), s2) == 0;
+}
+
+inline std::string trimSpaces(const std::string &input)
+{
+    size_t start = 0;
+    while (start < input.size() && input[start] == ' ')
+    {
+        start++;
+    }
+    size_t end = input.size();
+    while (end > start && (input[end - 1] == ' ' || input[end - 1] == '\n' || input[end - 1] == '\r'))
+    {
+        end--;
+    }
+    return input.substr(start, end - start);
+}
+
+inline std::string getExtension(const std::string& filename)
+{
+    auto pos = filename.find_last_of('.');
+    if (pos == std::string::npos)
+    {
+        return "";
+    }
+    return filename.substr( pos + 1);
+}
+
+template <typename T>
+void swapEndian(T &val)
+{
+    union U
+    {
+        T val;
+        std::array<std::uint8_t, sizeof(T)> raw;
+    } src, dst;
+
+    src.val = val;
+    std::reverse_copy(src.raw.begin(), src.raw.end(), dst.raw.begin());
+    val = dst.val;
+}
+
+ /** Checks if the value is a valid depth. For CV_16U or CV_16S, the convention is to be invalid if it is
+  * a limit. For a float/double, we just check if it is a NaN
+  * @param depth the depth to check for validity
+  */
+inline bool isValidDepth(const float& depth)
+{
+    return !cvIsNaN(depth);
+}
+
+inline bool isValidDepth(const double& depth)
+{
+    return !cvIsNaN(depth);
+}
+
+inline bool isValidDepth(const short int& depth)
+{
+    return (depth != std::numeric_limits<short int>::min()) &&
+           (depth != std::numeric_limits<short int>::max());
+}
+
+inline bool isValidDepth(const unsigned short int& depth)
+{
+    return (depth != std::numeric_limits<unsigned short int>::min()) &&
+           (depth != std::numeric_limits<unsigned short int>::max());
+}
+
+inline bool isValidDepth(const int& depth)
+{
+    return (depth != std::numeric_limits<int>::min()) &&
+           (depth != std::numeric_limits<int>::max());
+}
+
+inline bool isValidDepth(const unsigned int& depth)
+{
+    return (depth != std::numeric_limits<unsigned int>::min()) &&
+           (depth != std::numeric_limits<unsigned int>::max());
+}
+
+
+// One place to turn intrinsics on and off
+#define USE_INTRINSICS CV_SIMD128
+
+typedef float depthType;
+
+const float qnan = std::numeric_limits<float>::quiet_NaN();
+const cv::Vec3f nan3(qnan, qnan, qnan);
+#if USE_INTRINSICS
+const cv::v_float32x4 nanv(qnan, qnan, qnan, qnan);
+#endif
+
+inline bool isNaN(cv::Point3f p)
+{
+    return (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z));
+}
+
+#if USE_INTRINSICS
+static inline bool isNaN(const cv::v_float32x4& p)
+{
+    return cv::v_check_any(v_ne(p, p));
+}
+#endif
+
+inline size_t roundDownPow2(size_t x)
+{
+    size_t shift = 0;
+    while(x != 0)
+    {
+        shift++; x >>= 1;
+    }
+    return (size_t)(1ULL << (shift-1));
+}
+
+template<> class DataType<cv::Point3f>
+{
+public:
+    typedef float       value_type;
+    typedef value_type  work_type;
+    typedef value_type  channel_type;
+    typedef value_type  vec_type;
+    enum { generic_type = 0,
+           depth        = CV_32F,
+           channels     = 3,
+           fmt          = (int)'f',
+           type         = CV_MAKETYPE(depth, channels)
+         };
+};
+
+template<> class DataType<cv::Vec3f>
+{
+public:
+    typedef float       value_type;
+    typedef value_type  work_type;
+    typedef value_type  channel_type;
+    typedef value_type  vec_type;
+    enum { generic_type = 0,
+           depth        = CV_32F,
+           channels     = 3,
+           fmt          = (int)'f',
+           type         = CV_MAKETYPE(depth, channels)
+         };
+};
+
+template<> class DataType<cv::Vec4f>
+{
+public:
+    typedef float       value_type;
+    typedef value_type  work_type;
+    typedef value_type  channel_type;
+    typedef value_type  vec_type;
+    enum { generic_type = 0,
+           depth        = CV_32F,
+           channels     = 4,
+           fmt          = (int)'f',
+           type         = CV_MAKETYPE(depth, channels)
+         };
+};
+
+
+typedef cv::Vec4f ptype;
+inline cv::Vec3f fromPtype(const ptype& x)
+{
+    return cv::Vec3f(x[0], x[1], x[2]);
+}
+
+inline ptype toPtype(const cv::Vec3f& x)
+{
+    return ptype(x[0], x[1], x[2], 0);
+}
+
+enum
+{
+    DEPTH_TYPE = DataType<depthType>::type,
+    POINT_TYPE = DataType<ptype    >::type,
+    COLOR_TYPE = DataType<ptype    >::type
+};
+
+typedef cv::Mat_< ptype > Points;
+typedef Points Normals;
+typedef Points Colors;
+
+typedef cv::Point3f _ptype;
+typedef cv::Mat_< _ptype > _Points;
+typedef _Points _Normals;
+typedef _Points _Colors;
+
+enum
+{
+    _DEPTH_TYPE = DataType<depthType>::type,
+    _POINT_TYPE = DataType<_ptype   >::type,
+    _COLOR_TYPE = DataType<_ptype   >::type
+};
+
+typedef cv::Mat_< depthType > Depth;
+
+void makeFrameFromDepth(InputArray depth, OutputArray pyrPoints, OutputArray pyrNormals,
+                        const Matx33f intr, int levels, float depthFactor,
+                        float sigmaDepth, float sigmaSpatial, int kernelSize,
+                        float truncateThreshold);
+void buildPyramidPointsNormals(InputArray _points, InputArray _normals,
+                               OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
+                               int levels);
+
+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;
+};
+
+class OdometryFrame::Impl
+{
+public:
+    Impl() : pyramids(OdometryFramePyramidType::N_PYRAMIDS) { }
+    virtual ~Impl() {}
+
+    virtual void getImage(OutputArray image) const ;
+    virtual void getGrayImage(OutputArray image) const ;
+    virtual void getDepth(OutputArray depth) const ;
+    virtual void getProcessedDepth(OutputArray depth) const ;
+    virtual void getMask(OutputArray mask) const ;
+    virtual void getNormals(OutputArray normals) const ;
+
+    virtual int getPyramidLevels() const ;
+
+    virtual void getPyramidAt(OutputArray img,
+                              OdometryFramePyramidType pyrType, size_t level) const ;
+
+    UMat imageGray;
+    UMat image;
+    UMat depth;
+    UMat scaledDepth;
+    UMat mask;
+    UMat normals;
+    std::vector< std::vector<UMat> > pyramids;
+};
+
+} /* namespace cv */
+
+#endif

modules/ptcloud/src/volume_impl.cpp

@@ -0,0 +1,596 @@
+// 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
+
+
+#include <iostream>
+#include "volume_impl.hpp"
+#include "tsdf_functions.hpp"
+#include "hash_tsdf_functions.hpp"
+#include "color_tsdf_functions.hpp"
+#include "opencv2/imgproc.hpp"
+
+namespace cv
+{
+
+Volume::Impl::Impl(const VolumeSettings& _settings) :
+    settings(_settings)
+#ifdef HAVE_OPENCL
+    , useGPU(ocl::useOpenCL())
+#endif
+{}
+
+// TSDF
+
+TsdfVolume::TsdfVolume(const VolumeSettings& _settings) :
+    Volume::Impl(_settings)
+{
+    Vec3i volResolution;
+    settings.getVolumeResolution(volResolution);
+#ifndef HAVE_OPENCL
+    volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
+#else
+    if (useGPU)
+        gpu_volume = UMat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
+    else
+        cpu_volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
+#endif
+
+    reset();
+}
+TsdfVolume::~TsdfVolume() {}
+
+void TsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
+{
+    CV_TRACE_FUNCTION();
+#ifndef HAVE_OPENCL
+    Mat depth;
+#else
+    UMat depth;
+#endif
+    frame.getDepth(depth);
+    integrate(depth, _cameraPose);
+}
+
+void TsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
+{
+    CV_TRACE_FUNCTION();
+#ifndef HAVE_OPENCL
+    Mat depth = _depth.getMat();
+#else
+    UMat depth = _depth.getUMat();
+#endif
+    CV_Assert(!depth.empty());
+
+    Matx33f intr;
+    settings.getCameraIntegrateIntrinsics(intr);
+    Intr intrinsics(intr);
+    Vec6f newParams((float)depth.rows, (float)depth.cols,
+                    intrinsics.fx, intrinsics.fy,
+                    intrinsics.cx, intrinsics.cy);
+    if (!(frameParams == newParams))
+    {
+        frameParams = newParams;
+#ifndef HAVE_OPENCL
+        preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
+#else
+        if (useGPU)
+            ocl_preCalculationPixNorm(depth.size(), intrinsics, gpu_pixNorms);
+        else
+            preCalculationPixNorm(depth.size(), intrinsics, cpu_pixNorms);
+#endif
+    }
+    const Matx44f cameraPose = _cameraPose.getMat();
+
+#ifndef HAVE_OPENCL
+    integrateTsdfVolumeUnit(settings, cameraPose, depth, pixNorms, volume);
+#else
+    if (useGPU)
+        ocl_integrateTsdfVolumeUnit(settings, cameraPose, depth, gpu_pixNorms, gpu_volume);
+    else
+        integrateTsdfVolumeUnit(settings, cameraPose, depth, cpu_pixNorms, cpu_volume);
+#endif
+}
+void TsdfVolume::integrate(InputArray, InputArray, InputArray)
+{
+    CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+}
+
+
+void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors)  const
+{
+    Matx33f intr;
+    settings.getCameraRaycastIntrinsics(intr);
+    raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
+}
+
+
+void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
+{
+    if (_colors.needed())
+        CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+
+    CV_Assert(height > 0);
+    CV_Assert(width > 0);
+
+    const Matx44f cameraPose = _cameraPose.getMat();
+#ifndef HAVE_OPENCL
+    raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, volume, _points, _normals);
+#else
+    if (useGPU)
+        ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, gpu_volume, _points, _normals);
+    else
+        raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, cpu_volume, _points, _normals);
+#endif
+}
+
+void TsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
+{
+#ifndef HAVE_OPENCL
+    fetchNormalsFromTsdfVolumeUnit(settings, volume, points, normals);
+#else
+    if (useGPU)
+        ocl_fetchNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
+    else
+        fetchNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
+#endif
+}
+
+void TsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
+{
+#ifndef HAVE_OPENCL
+    fetchPointsNormalsFromTsdfVolumeUnit(settings, volume, points, normals);
+#else
+    if (useGPU)
+        ocl_fetchPointsNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
+    else
+        fetchPointsNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
+#endif
+}
+
+void TsdfVolume::fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
+{
+    CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+}
+
+void TsdfVolume::reset()
+{
+    CV_TRACE_FUNCTION();
+#ifndef HAVE_OPENCL
+    //TODO: use setTo(Scalar(0, 0))
+    volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
+        {
+            TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
+            v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+        });
+#else
+    if (useGPU)
+        gpu_volume.setTo(Scalar(floatToTsdf(0.0f), 0));
+    else
+        //TODO: use setTo(Scalar(0, 0))
+        cpu_volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
+            {
+                TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
+                v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+            });
+#endif
+}
+int TsdfVolume::getVisibleBlocks() const { return 1; }
+size_t TsdfVolume::getTotalVolumeUnits() const { return 1; }
+
+
+void TsdfVolume::getBoundingBox(OutputArray bb, int precision) const
+{
+    if (precision == Volume::BoundingBoxPrecision::VOXEL)
+    {
+        CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet");
+    }
+    else
+    {
+        float sz = this->settings.getVoxelSize();
+        Vec3f res;
+        this->settings.getVolumeResolution(res);
+        Vec3f volSize = res * sz;
+        Vec6f(0, 0, 0, volSize[0], volSize[1], volSize[2]).copyTo(bb);
+    }
+}
+
+void TsdfVolume::setEnableGrowth(bool /*v*/) { }
+
+bool TsdfVolume::getEnableGrowth() const
+{
+    return false;
+}
+
+
+// HASH_TSDF
+
+HashTsdfVolume::HashTsdfVolume(const VolumeSettings& _settings) :
+    Volume::Impl(_settings)
+{
+    Vec3i resolution;
+    settings.getVolumeResolution(resolution);
+    const Point3i volResolution = Point3i(resolution);
+    volumeUnitDegree = calcVolumeUnitDegree(volResolution);
+
+#ifndef HAVE_OPENCL
+    volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<TsdfVoxel>());
+    reset();
+#else
+    if (useGPU)
+    {
+        reset();
+    }
+    else
+    {
+        cpu_volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<TsdfVoxel>());
+        reset();
+    }
+#endif
+}
+
+HashTsdfVolume::~HashTsdfVolume() {}
+
+void HashTsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
+{
+    CV_TRACE_FUNCTION();
+#ifndef HAVE_OPENCL
+    Mat depth;
+#else
+    UMat depth;
+#endif
+    frame.getDepth(depth);
+    integrate(depth, _cameraPose);
+}
+
+void HashTsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
+{
+#ifndef HAVE_OPENCL
+    Mat depth = _depth.getMat();
+#else
+    UMat depth = _depth.getUMat();
+#endif
+    const Matx44f cameraPose = _cameraPose.getMat();
+    Matx33f intr;
+    settings.getCameraIntegrateIntrinsics(intr);
+    Intr intrinsics(intr);
+    Vec6f newParams((float)depth.rows, (float)depth.cols,
+        intrinsics.fx, intrinsics.fy,
+        intrinsics.cx, intrinsics.cy);
+    if (!(frameParams == newParams))
+    {
+        frameParams = newParams;
+#ifndef HAVE_OPENCL
+        preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
+#else
+        if (useGPU)
+            ocl_preCalculationPixNorm(depth.size(), intrinsics, gpu_pixNorms);
+        else
+            preCalculationPixNorm(depth.size(), intrinsics, cpu_pixNorms);
+#endif
+    }
+#ifndef HAVE_OPENCL
+    integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, enableGrowth, depth, pixNorms, volUnitsData, volumeUnits);
+    lastFrameId++;
+#else
+    if (useGPU)
+    {
+        ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, enableGrowth, depth, gpu_pixNorms,
+                                        lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags);
+    }
+    else
+    {
+        integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, enableGrowth, depth,
+                                    cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits);
+        lastFrameId++;
+    }
+#endif
+}
+
+void HashTsdfVolume::integrate(InputArray, InputArray, InputArray)
+{
+    CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+}
+
+
+void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors)  const
+{
+    Matx33f intr;
+    settings.getCameraRaycastIntrinsics(intr);
+    raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
+}
+
+
+void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
+{
+    if (_colors.needed())
+        CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+
+    const Matx44f cameraPose = _cameraPose.getMat();
+
+#ifndef HAVE_OPENCL
+    raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals);
+#else
+    if (useGPU)
+        ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals);
+    else
+        raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals);
+#endif
+}
+
+void HashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
+{
+#ifndef HAVE_OPENCL
+    fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
+#else
+    if (useGPU)
+        ocl_fetchNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
+    else
+        fetchNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
+
+#endif
+}
+void HashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
+{
+#ifndef HAVE_OPENCL
+    fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
+#else
+    if (useGPU)
+        ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
+    else
+        fetchPointsNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
+#endif
+}
+
+void HashTsdfVolume::fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
+{
+    CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+};
+
+void HashTsdfVolume::reset()
+{
+    CV_TRACE_FUNCTION();
+    lastVolIndex = 0;
+    lastFrameId = 0;
+    enableGrowth = true;
+#ifndef HAVE_OPENCL
+    volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
+        {
+            TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
+            v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+        });
+    volumeUnits = VolumeUnitIndexes();
+#else
+    if (useGPU)
+    {
+        Vec3i resolution;
+        settings.getVolumeResolution(resolution);
+
+        bufferSizeDegree = 15;
+        int buff_lvl = (int)(1 << bufferSizeDegree);
+        int volCubed = resolution[0] * resolution[1] * resolution[2];
+
+        volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType<TsdfVoxel>());
+        gpu_volUnitsData = cv::UMat(buff_lvl, volCubed, CV_8UC2);
+        lastVisibleIndices = cv::UMat(buff_lvl, 1, CV_32S);
+        isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U);
+        hashTable = CustomHashSet();
+        frameParams = Vec6f();
+        gpu_pixNorms = UMat();
+    }
+    else
+    {
+        cpu_volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
+            {
+                TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
+                v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+            });
+        cpu_volumeUnits = VolumeUnitIndexes();
+    }
+#endif
+}
+
+int HashTsdfVolume::getVisibleBlocks() const { return 1; }
+size_t HashTsdfVolume::getTotalVolumeUnits() const { return 1; }
+
+void HashTsdfVolume::setEnableGrowth(bool v)
+{
+    enableGrowth = v;
+}
+
+bool HashTsdfVolume::getEnableGrowth() const
+{
+    return enableGrowth;
+}
+
+void HashTsdfVolume::getBoundingBox(OutputArray boundingBox, int precision) const
+{
+    if (precision == Volume::BoundingBoxPrecision::VOXEL)
+    {
+        CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet");
+    }
+    else
+    {
+        Vec3i res;
+        this->settings.getVolumeResolution(res);
+        float voxelSize = this->settings.getVoxelSize();
+        float side = res[0] * voxelSize;
+
+        std::vector<Vec3i> vi;
+#ifndef HAVE_OPENCL
+        for (const auto& keyvalue : volumeUnits)
+        {
+            vi.push_back(keyvalue.first);
+        }
+#else
+        if (useGPU)
+        {
+            for (int row = 0; row < hashTable.last; row++)
+            {
+                Vec4i idx4 = hashTable.data[row];
+                vi.push_back(Vec3i(idx4[0], idx4[1], idx4[2]));
+            }
+        }
+        else
+        {
+            for (const auto& keyvalue : cpu_volumeUnits)
+            {
+                vi.push_back(keyvalue.first);
+            }
+        }
+#endif
+
+        if (vi.empty())
+        {
+            boundingBox.setZero();
+        }
+        else
+        {
+            std::vector<Point3f> pts;
+            for (Vec3i idx : vi)
+            {
+                Point3f base = Point3f((float)idx[0], (float)idx[1], (float)idx[2]) * side;
+                pts.push_back(base);
+                pts.push_back(base + Point3f(side, 0, 0));
+                pts.push_back(base + Point3f(0, side, 0));
+                pts.push_back(base + Point3f(0, 0, side));
+                pts.push_back(base + Point3f(side, side, 0));
+                pts.push_back(base + Point3f(side, 0, side));
+                pts.push_back(base + Point3f(0, side, side));
+                pts.push_back(base + Point3f(side, side, side));
+            }
+
+            const float mval = std::numeric_limits<float>::max();
+            Vec6f bb(mval, mval, mval, -mval, -mval, -mval);
+            for (auto p : pts)
+            {
+                // pt in local coords
+                Point3f pg = p;
+                bb[0] = min(bb[0], pg.x);
+                bb[1] = min(bb[1], pg.y);
+                bb[2] = min(bb[2], pg.z);
+                bb[3] = max(bb[3], pg.x);
+                bb[4] = max(bb[4], pg.y);
+                bb[5] = max(bb[5], pg.z);
+            }
+
+            bb.copyTo(boundingBox);
+        }
+    }
+}
+
+// COLOR_TSDF
+
+ColorTsdfVolume::ColorTsdfVolume(const VolumeSettings& _settings) :
+    Volume::Impl(_settings)
+{
+    Vec3i volResolution;
+    settings.getVolumeResolution(volResolution);
+    volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<RGBTsdfVoxel>());
+    reset();
+}
+
+ColorTsdfVolume::~ColorTsdfVolume() {}
+
+void ColorTsdfVolume::integrate(const OdometryFrame& frame, InputArray cameraPose)
+{
+    CV_TRACE_FUNCTION();
+    Mat depth;
+    frame.getDepth(depth);
+    Mat rgb;
+    frame.getImage(rgb);
+
+    integrate(depth, rgb, cameraPose);
+}
+
+void ColorTsdfVolume::integrate(InputArray, InputArray)
+{
+    CV_Error(cv::Error::StsBadFunc, "Color data should be passed for this volume type");
+}
+
+void ColorTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray _cameraPose)
+{
+    Mat depth = _depth.getMat();
+    Colors image = _image.getMat();
+    const Matx44f cameraPose = _cameraPose.getMat();
+    Matx33f intr;
+    settings.getCameraIntegrateIntrinsics(intr);
+    Intr intrinsics(intr);
+    Vec6f newParams((float)depth.rows, (float)depth.cols,
+        intrinsics.fx, intrinsics.fy,
+        intrinsics.cx, intrinsics.cy);
+    if (!(frameParams == newParams))
+    {
+        frameParams = newParams;
+        preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
+    }
+    integrateColorTsdfVolumeUnit(settings, cameraPose, depth, image, pixNorms, volume);
+}
+
+void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors)  const
+{
+    Matx33f intr;
+    settings.getCameraRaycastIntrinsics(intr);
+    raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
+}
+
+void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
+{
+    const Matx44f cameraPose = _cameraPose.getMat();
+    raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, intr, volume, _points, _normals, _colors);
+}
+
+void ColorTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
+{
+    fetchNormalsFromColorTsdfVolumeUnit(settings, volume, points, normals);
+}
+
+void ColorTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
+{
+    fetchPointsNormalsFromColorTsdfVolumeUnit(settings, volume, points, normals);
+}
+
+void ColorTsdfVolume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const
+{
+    fetchPointsNormalsColorsFromColorTsdfVolumeUnit(settings, volume, points, normals, colors);
+}
+
+void ColorTsdfVolume::reset()
+{
+    CV_TRACE_FUNCTION();
+
+    volume.forEach<VecRGBTsdfVoxel>([](VecRGBTsdfVoxel& vv, const int* /* position */)
+        {
+            RGBTsdfVoxel& v = reinterpret_cast<RGBTsdfVoxel&>(vv);
+            v.tsdf = floatToTsdf(0.0f); v.weight = 0;
+            v.r = v.g = v.b = 0;
+        });
+}
+
+int ColorTsdfVolume::getVisibleBlocks() const { return 1; }
+size_t ColorTsdfVolume::getTotalVolumeUnits() const { return 1; }
+
+void ColorTsdfVolume::getBoundingBox(OutputArray bb, int precision) const
+{
+    if (precision == Volume::BoundingBoxPrecision::VOXEL)
+    {
+        CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet");
+    }
+    else
+    {
+        float sz = this->settings.getVoxelSize();
+        Vec3f res;
+        this->settings.getVolumeResolution(res);
+        Vec3f volSize = res * sz;
+        Vec6f(0, 0, 0, volSize[0], volSize[1], volSize[2]).copyTo(bb);
+    }
+}
+
+void ColorTsdfVolume::setEnableGrowth(bool /*v*/) { }
+
+bool ColorTsdfVolume::getEnableGrowth() const
+{
+    return false;
+}
+
+}

modules/ptcloud/src/volume_impl.hpp

@@ -0,0 +1,257 @@
+// 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_IMPL_HPP
+#define OPENCV_3D_VOLUME_IMPL_HPP
+
+#include <iostream>
+
+#include "precomp.hpp"
+#include "hash_tsdf_functions.hpp"
+
+namespace cv
+{
+
+class Volume::Impl
+{
+private:
+    // TODO: make debug function, which show histogram of volume points values
+    // make this function run with debug lvl == 10
+public:
+    Impl(const VolumeSettings& settings);
+    virtual ~Impl() {};
+
+    virtual void integrate(const OdometryFrame& frame, InputArray pose) = 0;
+    virtual void integrate(InputArray depth, InputArray pose) = 0;
+    virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0;
+
+    virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
+    virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
+
+    virtual void fetchNormals(InputArray points, OutputArray normals) const = 0;
+    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
+    virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const = 0;
+
+    virtual void reset() = 0;
+    virtual int getVisibleBlocks() const = 0;
+    virtual size_t getTotalVolumeUnits() const = 0;
+
+    virtual void getBoundingBox(OutputArray bb, int precision) const = 0;
+    virtual void setEnableGrowth(bool v) = 0;
+    virtual bool getEnableGrowth() const = 0;
+
+public:
+    VolumeSettings settings;
+#ifdef HAVE_OPENCL
+    const bool useGPU;
+#endif
+};
+
+
+class TsdfVolume : public Volume::Impl
+{
+public:
+    TsdfVolume(const VolumeSettings& settings);
+    ~TsdfVolume();
+
+    virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
+    virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
+    virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void fetchNormals(InputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void reset() override;
+    virtual int getVisibleBlocks() const override;
+    virtual size_t getTotalVolumeUnits() const override;
+
+    // Gets bounding box in volume coordinates with given precision:
+    // VOLUME_UNIT - up to volume unit
+    // VOXEL - up to voxel
+    // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
+    virtual void getBoundingBox(OutputArray bb, int precision) const override;
+
+    // Enabels or disables new volume unit allocation during integration
+    // Applicable for HashTSDF only
+    virtual void setEnableGrowth(bool v) override;
+    // Returns if new volume units are allocated during integration or not
+    // Applicable for HashTSDF only
+    virtual bool getEnableGrowth() const override;
+
+public:
+    Vec6f frameParams;
+#ifndef HAVE_OPENCL
+    Mat pixNorms;
+    // See zFirstMemOrder arg of parent class constructor
+    // for the array layout info
+    // Consist of Voxel elements
+    Mat volume;
+#else
+    //temporary solution
+    Mat cpu_pixNorms;
+    Mat cpu_volume;
+    UMat gpu_pixNorms;
+    UMat gpu_volume;
+#endif
+};
+
+
+typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
+typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
+
+class HashTsdfVolume : public Volume::Impl
+{
+public:
+    HashTsdfVolume(const VolumeSettings& settings);
+    ~HashTsdfVolume();
+
+    virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
+    virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
+    virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void fetchNormals(InputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void reset() override;
+    virtual int getVisibleBlocks() const override;
+    virtual size_t getTotalVolumeUnits() const override;
+
+    // Enabels or disables new volume unit allocation during integration
+    // Applicable for HashTSDF only
+    virtual void setEnableGrowth(bool v) override;
+    // Returns if new volume units are allocated during integration or not
+    // Applicable for HashTSDF only
+    virtual bool getEnableGrowth() const override;
+
+    // Gets bounding box in volume coordinates with given precision:
+    // VOLUME_UNIT - up to volume unit
+    // VOXEL - up to voxel
+    // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
+    virtual void getBoundingBox(OutputArray bb, int precision) const override;
+
+public:
+    int lastVolIndex;
+    int lastFrameId;
+    Vec6f frameParams;
+    int volumeUnitDegree;
+    bool enableGrowth;
+
+#ifndef HAVE_OPENCL
+    Mat volUnitsData;
+    Mat pixNorms;
+    VolumeUnitIndexes volumeUnits;
+#else
+    VolumeUnitIndexes cpu_volumeUnits;
+
+    Mat cpu_volUnitsData;
+    Mat cpu_pixNorms;
+    UMat gpu_volUnitsData;
+    UMat gpu_pixNorms;
+
+    int bufferSizeDegree;
+    // per-volume-unit data
+    UMat lastVisibleIndices;
+    UMat isActiveFlags;
+    //TODO: remove it when there's no CPU parts
+    Mat volUnitsDataCopy;
+    //TODO: move indexes.volumes to GPU
+    CustomHashSet hashTable;
+#endif
+};
+
+
+class ColorTsdfVolume : public Volume::Impl
+{
+public:
+    ColorTsdfVolume(const VolumeSettings& settings);
+    ~ColorTsdfVolume();
+
+    virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray pose) override;
+    virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
+    virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
+    virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void fetchNormals(InputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
+    virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    virtual void reset() override;
+    virtual int getVisibleBlocks() const override;
+    virtual size_t getTotalVolumeUnits() const override;
+
+    // Gets bounding box in volume coordinates with given precision:
+    // VOLUME_UNIT - up to volume unit
+    // VOXEL - up to voxel
+    // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
+    virtual void getBoundingBox(OutputArray bb, int precision) const override;
+
+    // Enabels or disables new volume unit allocation during integration
+    // Applicable for HashTSDF only
+    virtual void setEnableGrowth(bool v) override;
+    // Returns if new volume units are allocated during integration or not
+    // Applicable for HashTSDF only
+    virtual bool getEnableGrowth() const override;
+
+private:
+    Vec4i volStrides;
+    Vec6f frameParams;
+    Mat pixNorms;
+    // See zFirstMemOrder arg of parent class constructor
+    // for the array layout info
+    // Consist of Voxel elements
+    Mat volume;
+};
+
+
+Volume::Volume(VolumeType vtype, const VolumeSettings& settings)
+{
+    switch (vtype)
+    {
+    case VolumeType::TSDF:
+        this->impl = makePtr<TsdfVolume>(settings);
+        break;
+    case VolumeType::HashTSDF:
+        this->impl = makePtr<HashTsdfVolume>(settings);
+        break;
+    case VolumeType::ColorTSDF:
+        this->impl = makePtr<ColorTsdfVolume>(settings);
+        break;
+    default:
+        CV_Error(Error::StsInternal, "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }");
+        break;
+    }
+}
+Volume::~Volume() {}
+
+void Volume::integrate(const OdometryFrame& frame, InputArray pose) { this->impl->integrate(frame, pose); }
+void Volume::integrate(InputArray depth, InputArray pose) { this->impl->integrate(depth, pose); }
+void Volume::integrate(InputArray depth, InputArray image, InputArray pose) { this->impl->integrate(depth, image, pose); }
+void Volume::raycast(InputArray cameraPose, OutputArray _points, OutputArray _normals) const { this->impl->raycast(cameraPose, _points, _normals, noArray()); }
+void Volume::raycast(InputArray cameraPose, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, _points, _normals, _colors); }
+void Volume::raycast(InputArray cameraPose, int height, int width, InputArray _intr, OutputArray _points, OutputArray _normals) const { this->impl->raycast(cameraPose, height, width, _intr, _points, _normals, noArray()); }
+void Volume::raycast(InputArray cameraPose, int height, int width, InputArray _intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _intr, _points, _normals, _colors); }
+
+void Volume::fetchNormals(InputArray points, OutputArray normals) const { this->impl->fetchNormals(points, normals); }
+void Volume::fetchPointsNormals(OutputArray points, OutputArray normals) const { this->impl->fetchPointsNormals(points, normals); }
+void Volume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const { this->impl->fetchPointsNormalsColors(points, normals, colors); };
+
+void Volume::reset() { this->impl->reset(); }
+int Volume::getVisibleBlocks() const { return this->impl->getVisibleBlocks(); }
+size_t Volume::getTotalVolumeUnits() const { return this->impl->getTotalVolumeUnits(); }
+
+void Volume::getBoundingBox(OutputArray bb, int precision) const { this->impl->getBoundingBox(bb, precision); }
+void Volume::setEnableGrowth(bool v) { this->impl->setEnableGrowth(v); }
+bool Volume::getEnableGrowth() const { return this->impl->getEnableGrowth(); }
+
+
+}
+
+#endif // !OPENCV_3D_VOLUME_IMPL_HPP

modules/ptcloud/src/volume_settings_impl.cpp

@@ -0,0 +1,535 @@
+// 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
+
+
+#include "precomp.hpp"
+
+namespace cv
+{
+
+static Vec4i calcVolumeStrides(Point3i volumeResolution, bool ZFirstMemOrder)
+{
+    // (xRes*yRes*zRes) array
+    // Depending on zFirstMemOrder arg:
+    // &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z;
+    // &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes;
+    int xdim, ydim, zdim;
+    if (ZFirstMemOrder)
+    {
+        xdim = volumeResolution.z * volumeResolution.y;
+        ydim = volumeResolution.z;
+        zdim = 1;
+    }
+    else
+    {
+        xdim = 1;
+        ydim = volumeResolution.x;
+        zdim = volumeResolution.x * volumeResolution.y;
+    }
+    return Vec4i(xdim, ydim, zdim);
+}
+
+class VolumeSettings::Impl
+{
+public:
+    Impl() {};
+    virtual ~Impl() {};
+
+    virtual void  setIntegrateWidth(int  val) = 0;
+    virtual int   getIntegrateWidth() const = 0;
+    virtual void  setIntegrateHeight(int  val) = 0;
+    virtual int   getIntegrateHeight() const = 0;
+    virtual void  setRaycastWidth(int  val) = 0;
+    virtual int   getRaycastWidth() const = 0;
+    virtual void  setRaycastHeight(int  val) = 0;
+    virtual int   getRaycastHeight() const = 0;
+    virtual void  setDepthFactor(float val) = 0;
+    virtual float getDepthFactor() const = 0;
+    virtual void  setVoxelSize(float  val) = 0;
+    virtual float getVoxelSize() const = 0;
+    virtual void  setTsdfTruncateDistance(float val) = 0;
+    virtual float getTsdfTruncateDistance() const = 0;
+    virtual void  setMaxDepth(float val) = 0;
+    virtual float getMaxDepth() const = 0;
+    virtual void  setMaxWeight(int val) = 0;
+    virtual int   getMaxWeight() const = 0;
+    virtual void  setRaycastStepFactor(float val) = 0;
+    virtual float getRaycastStepFactor() const = 0;
+
+    virtual void setVolumePose(InputArray val) = 0;
+    virtual void getVolumePose(OutputArray val) const = 0;
+    virtual void setVolumeResolution(InputArray val) = 0;
+    virtual void getVolumeResolution(OutputArray val) const = 0;
+    virtual void getVolumeStrides(OutputArray val) const = 0;
+    virtual void setCameraIntegrateIntrinsics(InputArray val) = 0;
+    virtual void getCameraIntegrateIntrinsics(OutputArray val) const = 0;
+    virtual void setCameraRaycastIntrinsics(InputArray val) = 0;
+    virtual void getCameraRaycastIntrinsics(OutputArray val) const = 0;
+};
+
+class VolumeSettingsImpl : public VolumeSettings::Impl
+{
+public:
+    VolumeSettingsImpl();
+    VolumeSettingsImpl(VolumeType volumeType);
+    ~VolumeSettingsImpl();
+
+    virtual void  setIntegrateWidth(int  val) override;
+    virtual int   getIntegrateWidth() const override;
+    virtual void  setIntegrateHeight(int  val) override;
+    virtual int   getIntegrateHeight() const override;
+    virtual void  setRaycastWidth(int  val) override;
+    virtual int   getRaycastWidth() const override;
+    virtual void  setRaycastHeight(int  val) override;
+    virtual int   getRaycastHeight() const override;
+    virtual void  setDepthFactor(float val) override;
+    virtual float getDepthFactor() const override;
+    virtual void  setVoxelSize(float  val) override;
+    virtual float getVoxelSize() const override;
+    virtual void  setTsdfTruncateDistance(float val) override;
+    virtual float getTsdfTruncateDistance() const override;
+    virtual void  setMaxDepth(float val) override;
+    virtual float getMaxDepth() const override;
+    virtual void  setMaxWeight(int val) override;
+    virtual int   getMaxWeight() const override;
+    virtual void  setRaycastStepFactor(float val) override;
+    virtual float getRaycastStepFactor() const override;
+
+    virtual void setVolumePose(InputArray val) override;
+    virtual void getVolumePose(OutputArray val) const override;
+    virtual void setVolumeResolution(InputArray val) override;
+    virtual void getVolumeResolution(OutputArray val) const override;
+    virtual void getVolumeStrides(OutputArray val) const override;
+    virtual void setCameraIntegrateIntrinsics(InputArray val) override;
+    virtual void getCameraIntegrateIntrinsics(OutputArray val) const override;
+    virtual void setCameraRaycastIntrinsics(InputArray val) override;
+    virtual void getCameraRaycastIntrinsics(OutputArray val) const override;
+
+private:
+    VolumeType volumeType;
+
+    int   integrateWidth;
+    int   integrateHeight;
+    int   raycastWidth;
+    int   raycastHeight;
+    float depthFactor;
+    float voxelSize;
+    float tsdfTruncateDistance;
+    float maxDepth;
+    int   maxWeight;
+    float raycastStepFactor;
+    bool  zFirstMemOrder;
+
+    Matx44f volumePose;
+    Point3i volumeResolution;
+    Vec4i volumeStrides;
+    Matx33f cameraIntegrateIntrinsics;
+    Matx33f cameraRaycastIntrinsics;
+
+public:
+    // duplicate classes for all volumes
+
+    class DefaultTsdfSets {
+    public:
+        static const int integrateWidth  = 640;
+        static const int integrateHeight = 480;
+        float ifx = 525.f; // focus point x axis
+        float ify = 525.f; // focus point y axis
+        float icx = float(integrateWidth) / 2.f - 0.5f;  // central point x axis
+        float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
+
+        static const int raycastWidth = 640;
+        static const int raycastHeight = 480;
+        float rfx = 525.f; // focus point x axis
+        float rfy = 525.f; // focus point y axis
+        float rcx = float(raycastWidth) / 2.f - 0.5f;  // central point x axis
+        float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
+
+        static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
+        static constexpr float volumeSize = 3.f; // meters
+        static constexpr float voxelSize = volumeSize / 128.f; //meters
+        static constexpr float tsdfTruncateDistance = 2 * voxelSize;
+        static constexpr float maxDepth = 0.f;
+        static const int maxWeight = 64; // number of frames
+        static constexpr float raycastStepFactor = 0.75f;
+        static const bool zFirstMemOrder = true; // order of voxels in volume
+
+        const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
+        const Matx44f  volumePoseMatrix = volumePose.matrix;
+        // Unlike original code, this should work with any volume size
+        // Not only when (x,y,z % 32) == 0
+        const Point3i  volumeResolution = Vec3i::all(128); //number of voxels
+    };
+
+    class DefaultHashTsdfSets {
+    public:
+        static const int integrateWidth = 640;
+        static const int integrateHeight = 480;
+        float ifx = 525.f; // focus point x axis
+        float ify = 525.f; // focus point y axis
+        float icx = float(integrateWidth) / 2.f - 0.5f;  // central point x axis
+        float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
+
+        static const int raycastWidth = 640;
+        static const int raycastHeight = 480;
+        float rfx = 525.f; // focus point x axis
+        float rfy = 525.f; // focus point y axis
+        float rcx = float(raycastWidth) / 2.f - 0.5f;  // central point x axis
+        float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
+
+        static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
+        static constexpr float volumeSize = 3.f; // meters
+        static constexpr float voxelSize = volumeSize / 512.f; //meters
+        static constexpr float tsdfTruncateDistance = 7 * voxelSize;
+        static constexpr float maxDepth = 4.f;
+        static const int maxWeight = 64; // number of frames
+        static constexpr float raycastStepFactor = 0.25f;
+        static const bool zFirstMemOrder = true; // order of voxels in volume
+
+        const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
+        const Matx44f  volumePoseMatrix = volumePose.matrix;
+        // Unlike original code, this should work with any volume size
+        // Not only when (x,y,z % 32) == 0
+        const Point3i  volumeResolution = Vec3i::all(16); //number of voxels
+    };
+
+    class DefaultColorTsdfSets {
+    public:
+        static const int integrateWidth = 640;
+        static const int integrateHeight = 480;
+        float ifx = 525.f; // focus point x axis
+        float ify = 525.f; // focus point y axis
+        float icx = float(integrateWidth) / 2.f - 0.5f;  // central point x axis
+        float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
+        float rgb_ifx = 525.f; // focus point x axis
+        float rgb_ify = 525.f; // focus point y axis
+        float rgb_icx = float(integrateWidth) / 2.f - 0.5f;  // central point x axis
+        float rgb_icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
+
+        static const int raycastWidth = 640;
+        static const int raycastHeight = 480;
+        float rfx = 525.f; // focus point x axis
+        float rfy = 525.f; // focus point y axis
+        float rcx = float(raycastWidth) / 2.f - 0.5f;  // central point x axis
+        float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
+        float rgb_rfx = 525.f; // focus point x axis
+        float rgb_rfy = 525.f; // focus point y axis
+        float rgb_rcx = float(raycastWidth) / 2.f - 0.5f;  // central point x axis
+        float rgb_rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
+        const Matx33f  cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
+
+        static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
+        static constexpr float volumeSize = 3.f; // meters
+        static constexpr float voxelSize = volumeSize / 128.f; //meters
+        static constexpr float tsdfTruncateDistance = 2 * voxelSize;
+        static constexpr float maxDepth = 0.f;
+        static const int maxWeight = 64; // number of frames
+        static constexpr float raycastStepFactor = 0.75f;
+        static const bool zFirstMemOrder = true; // order of voxels in volume
+
+        const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
+        const Matx44f  volumePoseMatrix = volumePose.matrix;
+        // Unlike original code, this should work with any volume size
+        // Not only when (x,y,z % 32) == 0
+        const Point3i  volumeResolution = Vec3i::all(128); //number of voxels
+    };
+
+};
+
+
+VolumeSettings::VolumeSettings(VolumeType volumeType)
+{
+    this->impl = makePtr<VolumeSettingsImpl>(volumeType);
+}
+
+VolumeSettings::VolumeSettings(const VolumeSettings& vs)
+{
+    this->impl = makePtr<VolumeSettingsImpl>(*vs.impl.dynamicCast<VolumeSettingsImpl>());
+}
+
+VolumeSettings& VolumeSettings::operator=(const VolumeSettings& vs)
+{
+    this->impl = makePtr<VolumeSettingsImpl>(*vs.impl.dynamicCast<VolumeSettingsImpl>());
+    return *this;
+}
+
+VolumeSettings::~VolumeSettings() {}
+
+void  VolumeSettings::setIntegrateWidth(int val) { this->impl->setIntegrateWidth(val); };
+int   VolumeSettings::getIntegrateWidth() const { return this->impl->getIntegrateWidth(); };
+void  VolumeSettings::setIntegrateHeight(int val) { this->impl->setIntegrateHeight(val); };
+int   VolumeSettings::getIntegrateHeight() const { return this->impl->getIntegrateHeight(); };
+void  VolumeSettings::setRaycastWidth(int val) { this->impl->setRaycastWidth(val); };
+int   VolumeSettings::getRaycastWidth() const { return this->impl->getRaycastWidth(); };
+void  VolumeSettings::setRaycastHeight(int val) { this->impl->setRaycastHeight(val); };
+int   VolumeSettings::getRaycastHeight() const { return this->impl->getRaycastHeight(); };
+void  VolumeSettings::setVoxelSize(float val) { this->impl->setVoxelSize(val); };
+float VolumeSettings::getVoxelSize() const { return this->impl->getVoxelSize(); };
+void  VolumeSettings::setRaycastStepFactor(float val) { this->impl->setRaycastStepFactor(val); };
+float VolumeSettings::getRaycastStepFactor() const { return this->impl->getRaycastStepFactor(); };
+void  VolumeSettings::setTsdfTruncateDistance(float val) { this->impl->setTsdfTruncateDistance(val); };
+float VolumeSettings::getTsdfTruncateDistance() const { return this->impl->getTsdfTruncateDistance(); };
+void  VolumeSettings::setMaxDepth(float val) { this->impl->setMaxDepth(val); };
+float VolumeSettings::getMaxDepth() const { return this->impl->getMaxDepth(); };
+void  VolumeSettings::setDepthFactor(float val) { this->impl->setDepthFactor(val); };
+float VolumeSettings::getDepthFactor() const { return this->impl->getDepthFactor(); };
+void  VolumeSettings::setMaxWeight(int val) { this->impl->setMaxWeight(val); };
+int   VolumeSettings::getMaxWeight() const { return this->impl->getMaxWeight(); };
+
+void VolumeSettings::setVolumePose(InputArray val) { this->impl->setVolumePose(val); };
+void VolumeSettings::getVolumePose(OutputArray val) const { this->impl->getVolumePose(val); };
+void VolumeSettings::setVolumeResolution(InputArray val) { this->impl->setVolumeResolution(val); };
+void VolumeSettings::getVolumeResolution(OutputArray val) const { this->impl->getVolumeResolution(val); };
+void VolumeSettings::getVolumeStrides(OutputArray val) const { this->impl->getVolumeStrides(val); };
+void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl->setCameraIntegrateIntrinsics(val); };
+void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); };
+void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); };
+void VolumeSettings::getCameraRaycastIntrinsics(OutputArray val) const { this->impl->getCameraRaycastIntrinsics(val); };
+
+
+VolumeSettingsImpl::VolumeSettingsImpl()
+    : VolumeSettingsImpl(VolumeType::TSDF)
+{
+}
+
+VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
+{
+    volumeType = _volumeType;
+    if (volumeType == VolumeType::TSDF)
+    {
+        DefaultTsdfSets ds = DefaultTsdfSets();
+
+        this->integrateWidth = ds.integrateWidth;
+        this->integrateHeight = ds.integrateHeight;
+        this->raycastWidth = ds.raycastWidth;
+        this->raycastHeight = ds.raycastHeight;
+        this->depthFactor = ds.depthFactor;
+        this->voxelSize = ds.voxelSize;
+        this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
+        this->maxDepth = ds.maxDepth;
+        this->maxWeight = ds.maxWeight;
+        this->raycastStepFactor = ds.raycastStepFactor;
+        this->zFirstMemOrder = ds.zFirstMemOrder;
+
+        this->volumePose = ds.volumePoseMatrix;
+        this->volumeResolution = ds.volumeResolution;
+        this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
+        this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
+        this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+    }
+    else if (volumeType == VolumeType::HashTSDF)
+    {
+        DefaultHashTsdfSets ds = DefaultHashTsdfSets();
+
+        this->integrateWidth = ds.integrateWidth;
+        this->integrateHeight = ds.integrateHeight;
+        this->raycastWidth = ds.raycastWidth;
+        this->raycastHeight = ds.raycastHeight;
+        this->depthFactor = ds.depthFactor;
+        this->voxelSize = ds.voxelSize;
+        this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
+        this->maxDepth = ds.maxDepth;
+        this->maxWeight = ds.maxWeight;
+        this->raycastStepFactor = ds.raycastStepFactor;
+        this->zFirstMemOrder = ds.zFirstMemOrder;
+
+        this->volumePose = ds.volumePoseMatrix;
+        this->volumeResolution = ds.volumeResolution;
+        this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
+        this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
+        this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+    }
+    else if (volumeType == VolumeType::ColorTSDF)
+    {
+        DefaultColorTsdfSets ds = DefaultColorTsdfSets();
+
+        this->integrateWidth = ds.integrateWidth;
+        this->integrateHeight = ds.integrateHeight;
+        this->raycastWidth = ds.raycastWidth;
+        this->raycastHeight = ds.raycastHeight;
+        this->depthFactor = ds.depthFactor;
+        this->voxelSize = ds.voxelSize;
+        this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
+        this->maxDepth = ds.maxDepth;
+        this->maxWeight = ds.maxWeight;
+        this->raycastStepFactor = ds.raycastStepFactor;
+        this->zFirstMemOrder = ds.zFirstMemOrder;
+
+        this->volumePose = ds.volumePoseMatrix;
+        this->volumeResolution = ds.volumeResolution;
+        this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
+        this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
+        this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
+    }
+}
+
+
+VolumeSettingsImpl::~VolumeSettingsImpl() {}
+
+
+void VolumeSettingsImpl::setIntegrateWidth(int val)
+{
+    this->integrateWidth = val;
+}
+
+int VolumeSettingsImpl::getIntegrateWidth() const
+{
+    return this->integrateWidth;
+}
+
+void VolumeSettingsImpl::setIntegrateHeight(int val)
+{
+    this->integrateHeight = val;
+}
+
+int VolumeSettingsImpl::getIntegrateHeight() const
+{
+    return this->integrateHeight;
+}
+
+void VolumeSettingsImpl::setRaycastWidth(int val)
+{
+    this->raycastWidth = val;
+}
+
+int VolumeSettingsImpl::getRaycastWidth() const
+{
+    return this->raycastWidth;
+}
+
+void VolumeSettingsImpl::setRaycastHeight(int val)
+{
+    this->raycastHeight = val;
+}
+
+int VolumeSettingsImpl::getRaycastHeight() const
+{
+    return this->raycastHeight;
+}
+
+void VolumeSettingsImpl::setDepthFactor(float val)
+{
+    this->depthFactor = val;
+}
+
+float VolumeSettingsImpl::getDepthFactor() const
+{
+    return this->depthFactor;
+}
+
+void VolumeSettingsImpl::setVoxelSize(float  val)
+{
+    this->voxelSize = val;
+}
+
+float VolumeSettingsImpl::getVoxelSize() const
+{
+    return this->voxelSize;
+}
+
+void VolumeSettingsImpl::setTsdfTruncateDistance(float val)
+{
+    this->tsdfTruncateDistance = val;
+}
+
+float VolumeSettingsImpl::getTsdfTruncateDistance() const
+{
+    return this->tsdfTruncateDistance;
+}
+
+void VolumeSettingsImpl::setMaxDepth(float val)
+{
+    this->maxDepth = val;
+}
+
+float VolumeSettingsImpl::getMaxDepth() const
+{
+    return this->maxDepth;
+}
+
+void VolumeSettingsImpl::setMaxWeight(int val)
+{
+    this->maxWeight = val;
+}
+
+int VolumeSettingsImpl::getMaxWeight() const
+{
+    return this->maxWeight;
+}
+
+void VolumeSettingsImpl::setRaycastStepFactor(float val)
+{
+    this->raycastStepFactor = val;
+}
+
+float VolumeSettingsImpl::getRaycastStepFactor() const
+{
+    return this->raycastStepFactor;
+}
+
+void VolumeSettingsImpl::setVolumePose(InputArray val)
+{
+    if (!val.empty())
+    {
+        val.copyTo(this->volumePose);
+    }
+}
+
+void VolumeSettingsImpl::getVolumePose(OutputArray val) const
+{
+    Mat(this->volumePose).copyTo(val);
+}
+
+void VolumeSettingsImpl::setVolumeResolution(InputArray val)
+{
+    if (!val.empty())
+    {
+        this->volumeResolution = Point3i(val.getMat());
+        this->volumeStrides = calcVolumeStrides(this->volumeResolution, this->zFirstMemOrder);
+    }
+}
+
+void VolumeSettingsImpl::getVolumeResolution(OutputArray val) const
+{
+    Mat(this->volumeResolution).copyTo(val);
+}
+
+void VolumeSettingsImpl::getVolumeStrides(OutputArray val) const
+{
+    Mat(this->volumeStrides).copyTo(val);
+}
+
+void VolumeSettingsImpl::setCameraIntegrateIntrinsics(InputArray val)
+{
+    if (!val.empty())
+    {
+        this->cameraIntegrateIntrinsics = Matx33f(val.getMat());
+    }
+}
+
+void VolumeSettingsImpl::getCameraIntegrateIntrinsics(OutputArray val) const
+{
+    Mat(this->cameraIntegrateIntrinsics).copyTo(val);
+}
+
+
+void VolumeSettingsImpl::setCameraRaycastIntrinsics(InputArray val)
+{
+    if (!val.empty())
+    {
+        this->cameraRaycastIntrinsics = Matx33f(val.getMat());
+    }
+}
+
+void VolumeSettingsImpl::getCameraRaycastIntrinsics(OutputArray val) const
+{
+    Mat(this->cameraRaycastIntrinsics).copyTo(val);
+}
+
+}

modules/ptcloud/test/test_main.cpp

@@ -0,0 +1,10 @@
+// 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.
+#include "test_precomp.hpp"
+
+#if defined(HAVE_HPX)
+    #include <hpx/hpx_main.hpp>
+#endif
+
+CV_TEST_MAIN("")

modules/ptcloud/test/test_normal.cpp

@@ -0,0 +1,725 @@
+// 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
+
+#include "test_precomp.hpp"
+#include <opencv2/geometry.hpp>
+#include <opencv2/core/quaternion.hpp>
+
+using namespace cv;
+
+namespace opencv_test { namespace {
+
+const int W = 640;
+const int H = 480;
+//int window_size = 5;
+float focal_length = 525;
+float cx = W / 2.f + 0.5f;
+float cy = H / 2.f + 0.5f;
+
+static Mat K() { static Mat res = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1); return res; }
+static Mat Kinv() { static Mat res = K().inv(); return res; }
+
+void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap);
+
+void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap)
+{
+    std::vector<Point3f> points3dvec;
+    for (int i = 0; i < H; i++)
+        for (int j = 0; j < W; j++)
+            points3dvec.push_back(Point3f(points3d(i, j)[0], points3d(i, j)[1], points3d(i, j)[2]));
+
+    std::vector<Point2f> img_points;
+    depthMap = Mat::zeros(H, W, CV_32F);
+    Vec3f R(0.0, 0.0, 0.0);
+    Vec3f T(0.0, 0.0, 0.0);
+    cv::projectPoints(points3dvec, R, T, K(), Mat(), img_points);
+
+    float maxv = 0.f;
+    int index = 0;
+    for (int i = 0; i < H; i++)
+    {
+
+        for (int j = 0; j < W; j++)
+        {
+            float value = (points3d(i, j))[2]; // value is the z
+            depthMap.at<float>(cvRound(img_points[index].y), cvRound(img_points[index].x)) = value;
+            maxv = std::max(maxv, value);
+            index++;
+        }
+    }
+
+    double scale = ((1 << 16) - 1) / maxv;
+    depthMap.convertTo(depthMap, CV_16U, scale);
+}
+
+
+struct Plane
+{
+public:
+    Vec4d nd;
+
+    Plane() : nd(1, 0, 0, 0) { }
+
+    static Plane generate(RNG& rng)
+    {
+        // Gaussian 3D distribution is separable and spherically symmetrical
+        // Being normalized, its points represent uniformly distributed points on a sphere (i.e. normal directions)
+        double sigma = 1.0;
+        Vec3d ngauss;
+        ngauss[0] = rng.gaussian(sigma);
+        ngauss[1] = rng.gaussian(sigma);
+        ngauss[2] = rng.gaussian(sigma);
+        ngauss = ngauss * (1.0 / cv::norm(ngauss));
+
+        double d = rng.uniform(-2.0, 2.0);
+        Plane p;
+        p.nd = Vec4d(ngauss[0], ngauss[1], ngauss[2], d);
+        return p;
+    }
+
+    Vec3d pixelIntersection(double u, double v, const Matx33d& K_inv)
+    {
+        Vec3d uv1(u, v, 1);
+        // pixel reprojected to camera space
+        Matx31d pspace = K_inv * uv1;
+
+        double d = this->nd[3];
+        double dotp = pspace.ddot({this->nd[0], this->nd[1], this->nd[2]});
+        double d_over_dotp = d / dotp;
+        if (std::fabs(dotp) <= 1e-9)
+        {
+            d_over_dotp = 1.0;
+            CV_LOG_INFO(NULL, "warning, dotp nearly 0! " << dotp);
+        }
+
+        Matx31d pmeet = pspace * (- d_over_dotp);
+        return {pmeet(0, 0), pmeet(1, 0), pmeet(2, 0)};
+    }
+};
+
+void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals,
+                   int n_planes, float scale, RNG& rng)
+{
+    const double minGoodZ = 0.0001;
+    const double maxGoodZ = 1000.0;
+
+    std::vector<Plane> planes;
+    for (int i = 0; i < n_planes; i++)
+    {
+        bool found = false;
+        for (int j = 0; j < 100; j++)
+        {
+            Plane px = Plane::generate(rng);
+
+            // Check that area corners have good z values
+            // So that they won't break rendering
+            double x0 = double(i) * double(W) / double(n_planes);
+            double x1 = double(i+1) * double(W) / double(n_planes);
+            std::vector<Point2d> corners = {{x0, 0}, {x0, H - 1}, {x1, 0}, {x1, H - 1}};
+            double minz = std::numeric_limits<double>::max();
+            double maxz = 0.0;
+            for (auto p : corners)
+            {
+                Vec3d v = px.pixelIntersection(p.x, p.y, Kinv());
+                minz = std::min(minz, v[2]);
+                maxz = std::max(maxz, v[2]);
+            }
+            if (minz > minGoodZ && maxz < maxGoodZ)
+            {
+                planes.push_back(px);
+                found = true;
+                break;
+            }
+        }
+        ASSERT_TRUE(found)  << "Failed to generate proper random plane" << std::endl;
+    }
+    Mat_ < Vec4f > outp(H, W);
+    Mat_ < Vec4f > outn(H, W);
+    plane_mask.create(H, W);
+
+    // n  ( r - r_0) = 0
+    // n * r_0 = d
+    //
+    // r_0 = (0,0,0)
+    // r[0]
+    for (int v = 0; v < H; v++)
+    {
+        for (int u = 0; u < W; u++)
+        {
+            unsigned int plane_index = (unsigned int)((u / float(W)) * planes.size());
+            Plane plane = planes[plane_index];
+            Vec3f pt = Vec3f(plane.pixelIntersection((double)u, (double)v, Kinv()) * scale);
+            outp(v, u) = {pt[0], pt[1], pt[2], 0};
+            outn(v, u) = {(float)plane.nd[0], (float)plane.nd[1], (float)plane.nd[2], 0};
+            plane_mask(v, u) = (uchar)plane_index;
+        }
+    }
+    planes_out = planes;
+    points3d = outp;
+    normals = outn;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+CV_ENUM(NormalComputers, RgbdNormals::RGBD_NORMALS_METHOD_FALS,
+                         RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD,
+                         RgbdNormals::RGBD_NORMALS_METHOD_SRI,
+                         RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT);
+typedef std::tuple<MatDepth, NormalComputers, bool, double, double, double, double, double> NormalsTestData;
+typedef std::tuple<NormalsTestData, int> NormalsTestParams;
+
+const double threshold3d1d = 1e-12;
+// Right angle is the maximum angle possible between two normals
+const double hpi = CV_PI / 2.0;
+const int nTestCasesNormals = 5;
+
+class NormalsRandomPlanes : public ::testing::TestWithParam<NormalsTestParams>
+{
+protected:
+    void SetUp() override
+    {
+        p = GetParam();
+        depth  = std::get<0>(std::get<0>(p));
+        alg = static_cast<RgbdNormals::RgbdNormalsMethod>(int(std::get<1>(std::get<0>(p))));
+        scale = std::get<2>(std::get<0>(p));
+        idx = std::get<1>(p);
+
+        float diffThreshold = scale ? 100000.f : 50.f;
+        normalsComputer = RgbdNormals::create(H, W, depth, K(), 5, diffThreshold, alg);
+        normalsComputer->cache();
+    }
+
+    struct NormalsCompareResult
+    {
+        double meanErr;
+        double maxErr;
+    };
+
+    static NormalsCompareResult checkNormals(Mat_<Vec4f> normals, Mat_<Vec4f> ground_normals)
+    {
+        double meanErr = 0, maxErr = 0;
+        for (int y = 0; y < normals.rows; ++y)
+        {
+            for (int x = 0; x < normals.cols; ++x)
+            {
+                Vec4f vec1 = normals(y, x), vec2 = ground_normals(y, x);
+                vec1 = vec1 / cv::norm(vec1);
+                vec2 = vec2 / cv::norm(vec2);
+
+                double dot = vec1.ddot(vec2);
+                // Just for rounding errors
+                double err = std::abs(dot) < 1.0 ? std::min(std::acos(dot), std::acos(-dot)) : 0.0;
+                meanErr += err;
+                maxErr = std::max(maxErr, err);
+            }
+        }
+        meanErr /= normals.rows * normals.cols;
+        return { meanErr, maxErr };
+    }
+
+    void runCase(bool scaleUp, int nPlanes, bool makeDepth,
+                 double meanThreshold, double maxThreshold, double threshold3d)
+    {
+        RNG& rng = cv::theRNG();
+        rng.state += idx + nTestCasesNormals*int(scale) + alg*16 + depth*64;
+
+        std::vector<Plane> plane_params;
+        Mat_<unsigned char> plane_mask;
+        Mat points3d, ground_normals;
+
+        gen_points_3d(plane_params, plane_mask, points3d, ground_normals, nPlanes, scaleUp ? 5000.f : 1.f, rng);
+
+        Mat in;
+        if (makeDepth)
+        {
+            points3dToDepth16U(points3d, in);
+        }
+        else
+        {
+            in = points3d;
+        }
+
+        TickMeter tm;
+        tm.start();
+        Mat in_normals, normals3d;
+        //TODO: check other methods when 16U input is implemented for them
+        if (normalsComputer->getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && in.channels() == 3)
+        {
+            std::vector<Mat> channels;
+            split(in, channels);
+            normalsComputer->apply(channels[2], in_normals);
+
+            normalsComputer->apply(in, normals3d);
+        }
+        else
+            normalsComputer->apply(in, in_normals);
+        tm.stop();
+
+        CV_LOG_INFO(NULL, "Speed: " << tm.getTimeMilli() << " ms");
+
+        Mat_<Vec4f> normals;
+        in_normals.convertTo(normals, CV_32FC4);
+
+        NormalsCompareResult res = checkNormals(normals, ground_normals);
+        double err3d = 0.0;
+        if (!normals3d.empty())
+        {
+            Mat_<Vec4f> cvtNormals3d;
+            normals3d.convertTo(cvtNormals3d, CV_32FC4);
+            err3d = checkNormals(cvtNormals3d, ground_normals).maxErr;
+        }
+
+        EXPECT_LE(res.meanErr, meanThreshold);
+        EXPECT_LE(res.maxErr, maxThreshold);
+        EXPECT_LE(err3d, threshold3d);
+    }
+
+    NormalsTestParams p;
+    int depth;
+    RgbdNormals::RgbdNormalsMethod alg;
+    bool scale;
+    int idx;
+
+    Ptr<RgbdNormals> normalsComputer;
+};
+
+//TODO Test NaNs in data
+
+TEST_P(NormalsRandomPlanes, check1plane)
+{
+    double meanErr = std::get<3>(std::get<0>(p));
+    double maxErr  = std::get<4>(std::get<0>(p));
+
+    // 1 plane, continuous scene, very low error..
+    runCase(scale, 1, false, meanErr, maxErr, threshold3d1d);
+}
+
+TEST_P(NormalsRandomPlanes, check3planes)
+{
+    double meanErr = std::get<5>(std::get<0>(p));
+    double maxErr  = hpi;
+
+    // 3 discontinuities, more error expected
+    runCase(scale, 3, false, meanErr, maxErr, threshold3d1d);
+}
+
+TEST_P(NormalsRandomPlanes, check1plane16u)
+{
+    // TODO: check other algos as soon as they support 16U depth inputs
+    if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && scale)
+    {
+        double meanErr = std::get<6>(std::get<0>(p));
+        double maxErr  = hpi;
+
+        runCase(false, 1, true, meanErr, maxErr, threshold3d1d);
+    }
+    else
+    {
+        throw SkipTestException("Not implemented for anything except LINEMOD with scale");
+    }
+}
+
+TEST_P(NormalsRandomPlanes, check3planes16u)
+{
+    // TODO: check other algos as soon as they support 16U depth inputs
+    if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && scale)
+    {
+        double meanErr = std::get<7>(std::get<0>(p));
+        double maxErr  = hpi;
+
+        runCase(false, 3, true, meanErr, maxErr, threshold3d1d);
+    }
+    else
+    {
+        throw SkipTestException("Not implemented for anything except LINEMOD with scale");
+    }
+}
+
+INSTANTIATE_TEST_CASE_P(RGBD_Normals, NormalsRandomPlanes,
+::testing::Combine(::testing::Values(
+    // 3 normal computer params + 5 thresholds:
+    //depth, alg, scale, 1plane mean, 1plane max, 3planes mean, 1plane16u mean, 3planes16 mean
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_FALS,  true, 0.00362, 0.08881, 0.02175, 0, 0},
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, false, 0.00374, 0.10309, 0.02, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_FALS,  true, 0.00023, 0.00037, 0.01805, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, false, 0.00023, 0.00037, 0.01805, 0, 0},
+
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD,  true, 0.00186, 0.08974, 0.04528, 0.21220, 0.17314},
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, false, 0.00157, 0.01225, 0.04528, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD,  true, 0.00160, 0.06526, 0.04371, 0.28837, 0.28918},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, false, 0.00154, 0.06877, 0.04323, 0, 0},
+
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI,  true, 0.01987, hpi, 0.036, 0, 0},
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01962, hpi, 0.037, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI,  true, 0.01958, hpi, 0.037, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01995, hpi, 0.036, 0, 0},
+
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT,  true, 0.000230, 0.00038, 0.00450, 0, 0},
+    NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, false, 0.000230, 0.00038, 0.00478, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT,  true, 0.000221, 0.00038, 0.00469, 0, 0},
+    NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, false, 0.000238, 0.00038, 0.00477, 0, 0}
+), ::testing::Range(0, nTestCasesNormals)));
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+typedef std::tuple<NormalComputers, std::pair<double, double>> NormalComputerThresholds;
+struct RenderedNormals: public ::testing::TestWithParam<std::tuple<MatDepth, NormalComputerThresholds, bool>>
+{
+    static Mat readYaml(std::string fname)
+    {
+        Mat img;
+        FileStorage fs(fname, FileStorage::Mode::READ);
+        if (fs.isOpened() && fs.getFirstTopLevelNode().name() == "testImg")
+        {
+            fs["testImg"] >> img;
+        }
+        return img;
+    };
+
+    static Mat nanMask(Mat img)
+    {
+        int depth = img.depth();
+        Mat mask(img.size(), CV_8U);
+        for (int y = 0; y < img.rows; y++)
+        {
+            uchar* maskRow = mask.ptr<uchar>(y);
+            if (depth == CV_32F)
+            {
+                Vec3f *imgrow = img.ptr<Vec3f>(y);
+                for (int x = 0; x < img.cols; x++)
+                {
+                    maskRow[x] = (imgrow[x] == imgrow[x])*255;
+                }
+            }
+            else if (depth == CV_64F)
+            {
+                Vec3d *imgrow = img.ptr<Vec3d>(y);
+                for (int x = 0; x < img.cols; x++)
+                {
+                    maskRow[x] = (imgrow[x] == imgrow[x])*255;
+                }
+            }
+        }
+        return mask;
+    }
+
+    template<typename VT>
+    static Mat flipAxesT(Mat pts, int flip)
+    {
+        Mat flipped(pts.size(), pts.type());
+        for (int y = 0; y < pts.rows; y++)
+        {
+            VT *inrow = pts.ptr<VT>(y);
+            VT *outrow = flipped.ptr<VT>(y);
+            for (int x = 0; x < pts.cols; x++)
+            {
+                VT n = inrow[x];
+                n[0] = (flip & FLIP_X) ? -n[0] : n[0];
+                n[1] = (flip & FLIP_Y) ? -n[1] : n[1];
+                n[2] = (flip & FLIP_Z) ? -n[2] : n[2];
+                outrow[x] = n;
+            }
+        }
+        return flipped;
+    }
+
+    static const int FLIP_X = 1;
+    static const int FLIP_Y = 2;
+    static const int FLIP_Z = 4;
+    static Mat flipAxes(Mat pts, int flip)
+    {
+        int depth = pts.depth();
+        if (depth == CV_32F)
+        {
+            return flipAxesT<Vec3f>(pts, flip);
+        }
+        else if (depth == CV_64F)
+        {
+            return flipAxesT<Vec3d>(pts, flip);
+        }
+        else
+        {
+            return Mat();
+        }
+    }
+
+    template<typename VT>
+    static Mat_<typename VT::value_type> normalsErrorT(Mat_<VT> srcNormals, Mat_<VT> dstNormals)
+    {
+        typedef typename VT::value_type Val;
+        Mat out(srcNormals.size(), cv::traits::Depth<Val>::value, Scalar(0));
+        for (int y = 0; y < srcNormals.rows; y++)
+        {
+
+            VT *srcrow = srcNormals[y];
+            VT *dstrow = dstNormals[y];
+            Val *outrow = out.ptr<Val>(y);
+            for (int x = 0; x < srcNormals.cols; x++)
+            {
+                VT sn = srcrow[x];
+                VT dn = dstrow[x];
+
+                Val dot = sn.dot(dn);
+                Val v(0.0);
+                // Just for rounding errors
+                if (std::abs(dot) < 1)
+                    v = std::min(std::acos(dot), std::acos(-dot));
+
+                outrow[x] = v;
+            }
+        }
+        return out;
+    }
+
+    static Mat normalsError(Mat srcNormals, Mat dstNormals)
+    {
+        int depth = srcNormals.depth();
+        int channels = srcNormals.channels();
+
+        if (depth == CV_32F)
+        {
+            if (channels == 3)
+            {
+                return normalsErrorT<Vec3f>(srcNormals, dstNormals);
+            }
+            else if (channels == 4)
+            {
+                return normalsErrorT<Vec4f>(srcNormals, dstNormals);
+            }
+        }
+        else if (depth == CV_64F)
+        {
+            if (channels == 3)
+            {
+                return normalsErrorT<Vec3d>(srcNormals, dstNormals);
+            }
+            else if (channels == 4)
+            {
+                return normalsErrorT<Vec4d>(srcNormals, dstNormals);
+            }
+        }
+        else
+        {
+            CV_Error(Error::StsInternal, "This type is unsupported");
+        }
+        return Mat();
+    }
+};
+
+
+TEST_P(RenderedNormals, check)
+{
+    auto p = GetParam();
+    int depth  = std::get<0>(p);
+    auto alg = static_cast<RgbdNormals::RgbdNormalsMethod>(int(std::get<0>(std::get<1>(p))));
+    bool scale = std::get<2>(p);
+
+    std::string dataPath = cvtest::TS::ptr()->get_data_path();
+    // The depth rendered from scene OPENCV_TEST_DATA_PATH + "/cv/rgbd/normals_check/normals_scene.blend"
+    std::string srcDepthFilename = dataPath + "/cv/rgbd/normals_check/depth.yaml.gz";
+    std::string srcNormalsFilename = dataPath + "/cv/rgbd/normals_check/normals%d.yaml.gz";
+    Mat srcDepth = readYaml(srcDepthFilename);
+
+    ASSERT_FALSE(srcDepth.empty()) << "Failed to load depth data";
+
+    Size depthSize = srcDepth.size();
+
+    Mat srcNormals;
+    std::array<Mat, 3> srcNormalsCh;
+    for (int i = 0; i < 3; i++)
+    {
+        Mat m = readYaml(cv::format(srcNormalsFilename.c_str(), i));
+
+        ASSERT_FALSE(m.empty()) << "Failed to load normals data";
+
+        if (depth == CV_64F)
+        {
+            Mat c;
+            m.convertTo(c, CV_64F);
+            m = c;
+        }
+
+        srcNormalsCh[i] = m;
+    }
+    cv::merge(srcNormalsCh, srcNormals);
+
+    // Convert saved normals from [0; 1] range to [-1; 1]
+    srcNormals = srcNormals * 2.0 - 1.0;
+
+    // Data obtained from Blender scene
+    Matx33f intr(666.6667f, 0.f, 320.f,
+                 0.f, 666.6667f, 240.f,
+                 0.f, 0.f, 1.f);
+    // Inverted camera rotation
+    Matx33d rotm = cv::Quatd(0.7805, 0.4835, 0.2087, 0.3369).conjugate().toRotMat3x3();
+    cv::transform(srcNormals, srcNormals, rotm);
+
+    Mat srcMask = srcDepth > 0;
+
+    float diffThreshold = 50.f;
+    if (scale)
+    {
+        srcDepth = srcDepth * 5000.0;
+        diffThreshold = 100000.f;
+    }
+
+    Mat srcCloud;
+    // The function with mask produces 1x(w*h) vector, this is not what we need
+    // depthTo3d(srcDepth, intr, srcCloud, srcMask);
+    depthTo3d(srcDepth, intr, srcCloud);
+    Scalar qnan = Scalar::all(std::numeric_limits<double>::quiet_NaN());
+    srcCloud.setTo(qnan, ~srcMask);
+    srcDepth.setTo(qnan, ~srcMask);
+
+    // For further result comparison
+    srcNormals.setTo(qnan, ~srcMask);
+
+    Ptr<RgbdNormals> normalsComputer = RgbdNormals::create(depthSize.height, depthSize.width, depth, intr, 5, diffThreshold, alg);
+    normalsComputer->cache();
+
+    Mat dstNormals, dstNormalsOrig, dstNormalsDepth;
+    normalsComputer->apply(srcCloud, dstNormals);
+    //TODO: add for other methods too when it's implemented
+    if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
+    {
+        normalsComputer->apply(srcDepth, dstNormalsDepth);
+        dstNormalsOrig = dstNormals.clone();
+    }
+
+    // Remove 4th channel from dstNormals
+    Mat newDstNormals;
+    std::vector<Mat> dstNormalsCh;
+    split(dstNormals, dstNormalsCh);
+    dstNormalsCh.resize(3);
+    merge(dstNormalsCh, newDstNormals);
+    dstNormals = newDstNormals;
+
+    Mat dstMask = nanMask(dstNormals);
+    // div by 8 because uchar is 8-bit
+    double maskl2 = cv::norm(dstMask, srcMask, NORM_HAMMING) / 8;
+
+    // Flipping Y and Z to correspond to srcNormals
+    Mat flipped = flipAxes(dstNormals, FLIP_Y | FLIP_Z);
+    dstNormals = flipped;
+
+    Mat absdot = normalsError(srcNormals, dstNormals);
+
+    Mat cmpMask = srcMask & dstMask;
+
+    EXPECT_GT(countNonZero(cmpMask), 0);
+
+    double nrml2 = cv::norm(absdot, NORM_L2, cmpMask);
+
+    if (!dstNormalsDepth.empty())
+    {
+        Mat abs3d = normalsError(dstNormalsOrig, dstNormalsDepth);
+        double errInf = cv::norm(abs3d, NORM_INF, cmpMask);
+        double errL2 = cv::norm(abs3d, NORM_L2, cmpMask);
+        EXPECT_LE(errInf, 0.00085);
+        EXPECT_LE(errL2, 0.07718);
+    }
+
+    auto th = std::get<1>(std::get<1>(p));
+    EXPECT_LE(nrml2,  th.first);
+    EXPECT_LE(maskl2, th.second);
+}
+
+INSTANTIATE_TEST_CASE_P(RGBD_Normals, RenderedNormals, ::testing::Combine(::testing::Values(CV_32F, CV_64F),
+                                                                          ::testing::Values(
+    NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_FALS,          {  81.8213,     0}},
+    NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD,       { 107.2710, 29168}},
+    NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_SRI,           {  73.2027, 17693}},
+    NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, {  57.9832,  2531}}),
+                                                                          ::testing::Values(true, false)));
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class RgbdPlaneGenerate : public ::testing::TestWithParam<std::tuple<int, bool, int>>
+{
+protected:
+    void SetUp() override
+    {
+        auto p = GetParam();
+        idx = std::get<0>(p);
+        checkNormals = std::get<1>(p);
+        nPlanes = std::get<2>(p);
+    }
+
+    int idx;
+    bool checkNormals;
+    int nPlanes;
+};
+
+TEST_P(RgbdPlaneGenerate, compute)
+{
+    RNG &rng = cvtest::TS::ptr()->get_rng();
+    rng.state += idx;
+
+    std::vector<Plane> planes;
+    Mat points3d, ground_normals;
+    Mat_<unsigned char> gt_plane_mask;
+    gen_points_3d(planes, gt_plane_mask, points3d, ground_normals, nPlanes, 1.f, rng);
+
+    Mat plane_mask;
+    std::vector<Vec4f> plane_coefficients;
+
+    Mat normals;
+    if (checkNormals)
+    {
+        // First, get the normals
+        int depth = CV_32F;
+        Ptr<RgbdNormals> normalsComputer = RgbdNormals::create(H, W, depth, K(), 5, 50.f, RgbdNormals::RGBD_NORMALS_METHOD_FALS);
+        normalsComputer->apply(points3d, normals);
+    }
+
+    findPlanes(points3d, normals, plane_mask, plane_coefficients);
+
+    // Compare each found plane to each ground truth plane
+    int n_planes = (int)plane_coefficients.size();
+    int n_gt_planes = (int)planes.size();
+    Mat_<int> matching(n_gt_planes, n_planes);
+    for (int j = 0; j < n_gt_planes; ++j)
+    {
+        Mat gt_mask = gt_plane_mask == j;
+        int n_gt = countNonZero(gt_mask);
+        int n_max = 0, i_max = 0;
+        for (int i = 0; i < n_planes; ++i)
+        {
+            Mat dst;
+            bitwise_and(gt_mask, plane_mask == i, dst);
+            matching(j, i) = countNonZero(dst);
+            if (matching(j, i) > n_max)
+            {
+                n_max = matching(j, i);
+                i_max = i;
+            }
+        }
+        // Get the best match
+        ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001);
+        // Compare the normals
+        Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]);
+        Vec4d nd = planes[j].nd;
+        ASSERT_GE(std::abs(Vec3d(nd[0], nd[1], nd[2]).dot(normal)), 0.95);
+    }
+}
+
+// 1 plane, continuous scene, very low error
+// 3 planes, 3 discontinuities, more error expected
+INSTANTIATE_TEST_CASE_P(RGBD_Plane, RgbdPlaneGenerate, ::testing::Combine(::testing::Range(0, 10),
+                                                                            ::testing::Values(false, true),
+                                                                            ::testing::Values(1, 3)));
+
+TEST(RGBD_Plane, regression2309ValgrindCheck)
+{
+    Mat points(640, 480, CV_32FC3, Scalar::all(0));
+    // Note, 640%9 is 1 and 480%9 is 3
+    int blockSize = 9;
+
+    Mat mask;
+    std::vector<cv::Vec4f> planes;
+    // Will corrupt memory; valgrind gets triggered
+    findPlanes(points, noArray(), mask, planes, blockSize);
+}
+
+}} // namespace

modules/ptcloud/test/test_octree.cpp

@@ -0,0 +1,158 @@
+// 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.
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+using namespace cv;
+
+class OctreeTest: public testing::Test
+{
+protected:
+    void SetUp() override
+    {
+        pointCloudSize = 1000;
+        resolution = 0.0001;
+        int scale;
+        Point3i pmin, pmax;
+        scale = 1 << 20;
+        pmin = Point3i(-scale, -scale, -scale);
+        pmax = Point3i(scale, scale, scale);
+
+        RNG& rng_Point = theRNG(); // set random seed for fixing output 3D point.
+
+        // Generate 3D PointCloud
+        for(size_t i = 0; i < pointCloudSize; i++)
+        {
+            float _x = 10 * (float)rng_Point.uniform(pmin.x, pmax.x)/scale;
+            float _y = 10 * (float)rng_Point.uniform(pmin.y, pmax.y)/scale;
+            float _z = 10 * (float)rng_Point.uniform(pmin.z, pmax.z)/scale;
+            pointCloud.push_back(Point3f(_x, _y, _z));
+        }
+
+        // Generate Octree From PointCloud
+        treeTest = Octree::createWithResolution(resolution, pointCloud);
+
+        // Randomly generate another 3D point.
+        float _x = 10 * (float)rng_Point.uniform(pmin.x, pmax.x)/scale;
+        float _y = 10 * (float)rng_Point.uniform(pmin.y, pmax.y)/scale;
+        float _z = 10 * (float)rng_Point.uniform(pmin.z, pmax.z)/scale;
+        restPoint = Point3f(_x, _y, _z);
+    }
+
+public:
+    //Origin point cloud data
+    std::vector<Point3f> pointCloud;
+
+    //Point cloud data from octree
+    std::vector<Point3f> restorePointCloudData;
+
+    //Color attribute of pointCloud from octree
+    std::vector<Point3f> restorePointCloudColor;
+
+    size_t pointCloudSize;
+    Point3f restPoint;
+    Ptr<Octree> treeTest;
+    double resolution;
+};
+
+TEST_F(OctreeTest, BasicFunctionTest)
+{
+    // Check if the point in Bound.
+    EXPECT_TRUE(treeTest->isPointInBound(restPoint));
+    EXPECT_FALSE(treeTest->isPointInBound(restPoint + Point3f(60, 60, 60)));
+
+    // insert, delete Test.
+    EXPECT_FALSE(treeTest->deletePoint(restPoint));
+    EXPECT_FALSE(treeTest->insertPoint(restPoint + Point3f(60, 60, 60)));
+    EXPECT_TRUE(treeTest->insertPoint(restPoint));
+    EXPECT_TRUE(treeTest->deletePoint(restPoint));
+
+    EXPECT_FALSE(treeTest->empty());
+    EXPECT_NO_THROW(treeTest->clear());
+    EXPECT_TRUE(treeTest->empty());
+}
+
+TEST_F(OctreeTest, RadiusSearchTest)
+{
+    float radius = 2.0f;
+    std::vector<Point3f> outputPoints;
+    std::vector<float> outputSquareDist;
+    EXPECT_NO_THROW(treeTest->radiusNNSearch(restPoint, radius, outputPoints, outputSquareDist));
+    EXPECT_EQ(outputPoints.size(),(unsigned int)5);
+
+    // The output is unsorted, so let's sort it before checking
+    std::map<float, Point3f> sortResults;
+    for (int i = 0; i < (int)outputPoints.size(); i++)
+    {
+        sortResults[outputSquareDist[i]] = outputPoints[i];
+    }
+
+    std::vector<Point3f> goldVals = {
+        {-8.1184864044189f, -0.528564453125f, 0.f},
+        {-8.405818939208f,  -2.991247177124f, 0.f},
+        {-8.88461112976f,   -1.881799697875f, 0.f},
+        {-6.551313400268f,  -0.708484649658f, 0.f}
+    };
+
+    auto it = sortResults.begin();
+    for (int i = 0; i < (int)goldVals.size(); i++, it++)
+    {
+        Point3f p = it->second;
+        EXPECT_FLOAT_EQ(goldVals[i].x, p.x);
+        EXPECT_FLOAT_EQ(goldVals[i].y, p.y);
+    }
+}
+
+TEST_F(OctreeTest, KNNSearchTest)
+{
+    int K = 10;
+    std::vector<Point3f> outputPoints;
+    std::vector<float> outputSquareDist;
+    EXPECT_NO_THROW(treeTest->KNNSearch(restPoint, K, outputPoints, outputSquareDist));
+
+    // The output is unsorted, so let's sort it before checking
+    std::map<float, Point3f> sortResults;
+    for (int i = 0; i < (int)outputPoints.size(); i++)
+    {
+        sortResults[outputSquareDist[i]] = outputPoints[i];
+    }
+
+    std::vector<Point3f> goldVals = {
+        { -8.118486404418f, -0.528564453125f, 0.f },
+        { -8.405818939208f, -2.991247177124f, 0.f },
+        { -8.884611129760f, -1.881799697875f, 0.f },
+        { -6.551313400268f, -0.708484649658f, 0.f }
+    };
+
+    auto it = sortResults.begin();
+    for (int i = 0; i < (int)goldVals.size(); i++, it++)
+    {
+        Point3f p = it->second;
+        EXPECT_FLOAT_EQ(goldVals[i].x, p.x);
+        EXPECT_FLOAT_EQ(goldVals[i].y, p.y);
+    }
+}
+
+TEST_F(OctreeTest, restoreTest) {
+    //restore the pointCloud data from octree.
+    EXPECT_NO_THROW(treeTest->getPointCloudByOctree(restorePointCloudData,restorePointCloudColor));
+
+    //The points in same leaf node will be seen as the same point. So if the resolution is small,
+    //it will work as a downSampling function.
+    EXPECT_LE(restorePointCloudData.size(),pointCloudSize);
+
+    //The distance between the restore point cloud data and origin data should less than resolution.
+    std::vector<Point3f> outputPoints;
+    std::vector<float> outputSquareDist;
+    EXPECT_NO_THROW(treeTest->getPointCloudByOctree(restorePointCloudData,restorePointCloudColor));
+    EXPECT_NO_THROW(treeTest->KNNSearch(restorePointCloudData[0], 1, outputPoints, outputSquareDist));
+    EXPECT_LE(abs(outputPoints[0].x - restorePointCloudData[0].x), resolution);
+    EXPECT_LE(abs(outputPoints[0].y - restorePointCloudData[0].y), resolution);
+    EXPECT_LE(abs(outputPoints[0].z - restorePointCloudData[0].z), resolution);
+}
+
+} // namespace
+} // opencv_test

modules/ptcloud/test/test_odometry.cpp

@@ -0,0 +1,833 @@
+// 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
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+static
+void dilateFrame(Mat& image, Mat& depth)
+{
+    CV_Assert(!image.empty());
+    CV_Assert(image.type() == CV_8UC1);
+
+    CV_Assert(!depth.empty());
+    CV_Assert(depth.type() == CV_32FC1);
+    CV_Assert(depth.size() == image.size());
+
+    Mat mask;
+    inRange(depth, FLT_EPSILON, 10, mask);
+
+    image.setTo(255, ~mask);
+    Mat minImage;
+    erode(image, minImage, Mat());
+
+    image.setTo(0, ~mask);
+    Mat maxImage;
+    dilate(image, maxImage, Mat());
+
+    depth.setTo(FLT_MAX, ~mask);
+    Mat minDepth;
+    erode(depth, minDepth, Mat());
+
+    depth.setTo(0, ~mask);
+    Mat maxDepth;
+    dilate(depth, maxDepth, Mat());
+
+    Mat dilatedMask;
+    dilate(mask, dilatedMask, Mat(), Point(-1,-1), 1);
+    for(int y = 0; y < depth.rows; y++)
+        for(int x = 0; x < depth.cols; x++)
+            if(!mask.at<uchar>(y,x) && dilatedMask.at<uchar>(y,x))
+            {
+                image.at<uchar>(y,x) = static_cast<uchar>(0.5f * (static_cast<float>(minImage.at<uchar>(y,x)) +
+                                                                  static_cast<float>(maxImage.at<uchar>(y,x))));
+                depth.at<float>(y,x) = 0.5f * (minDepth.at<float>(y,x) + maxDepth.at<float>(y,x));
+            }
+}
+
+class OdometryTest
+{
+public:
+    OdometryTest(OdometryType _otype,
+                 OdometryAlgoType _algtype,
+                 double _maxError1,
+                 double _maxError5,
+                 double _idError = DBL_EPSILON) :
+        otype(_otype),
+        algtype(_algtype),
+        maxError1(_maxError1),
+        maxError5(_maxError5),
+        idError(_idError)
+    { }
+
+    void readData(Mat& image, Mat& depth) const;
+    static Mat getCameraMatrix()
+    {
+        float fx = 525.0f, // default
+              fy = 525.0f,
+              cx = 319.5f,
+              cy = 239.5f;
+        Matx33f K(fx,  0, cx,
+                   0, fy, cy,
+                   0,  0,  1);
+        return Mat(K);
+    }
+    static void generateRandomTransformation(Mat& R, Mat& t);
+
+    void run();
+    void checkUMats();
+    void prepareFrameCheck();
+    void processedDepthCheck();
+
+    OdometryType otype;
+    OdometryAlgoType algtype;
+    double maxError1;
+    double maxError5;
+    double idError;
+};
+
+
+void OdometryTest::readData(Mat& image, Mat& depth) const
+{
+    std::string dataPath = cvtest::TS::ptr()->get_data_path();
+    std::string imageFilename = dataPath + "/cv/rgbd/rgb.png";
+    std::string depthFilename = dataPath + "/cv/rgbd/depth.png";
+
+    image = imread(imageFilename,  0);
+    depth = imread(depthFilename, -1);
+
+    ASSERT_FALSE(image.empty()) << "Image " << imageFilename.c_str() << " can not be read" << std::endl;
+    ASSERT_FALSE(depth.empty()) << "Depth " << depthFilename.c_str() << "can not be read" << std::endl;
+
+    CV_DbgAssert(image.type() == CV_8UC1);
+    CV_DbgAssert(depth.type() == CV_16UC1);
+    {
+        Mat depth_flt;
+        depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
+        depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth_flt < FLT_EPSILON);
+        depth = depth_flt;
+    }
+}
+
+void OdometryTest::generateRandomTransformation(Mat& rvec, Mat& tvec)
+{
+    const float maxRotation = (float)(3.f / 180.f * CV_PI); //rad
+    const float maxTranslation = 0.02f; //m
+
+    RNG& rng = theRNG();
+    rvec.create(3, 1, CV_64FC1);
+    tvec.create(3, 1, CV_64FC1);
+
+    randu(rvec, Scalar(-1000), Scalar(1000));
+    normalize(rvec, rvec, rng.uniform(0.007f, maxRotation));
+
+    randu(tvec, Scalar(-1000), Scalar(1000));
+    normalize(tvec, tvec, rng.uniform(0.008f, maxTranslation));
+}
+
+void OdometryTest::checkUMats()
+{
+    Mat K = getCameraMatrix();
+
+    Mat image, depth;
+    readData(image, depth);
+
+    UMat uimage, udepth;
+    image.copyTo(uimage);
+    depth.copyTo(udepth);
+
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf(udepth, uimage);
+
+    Mat calcRt;
+
+    uimage.release();
+    udepth.release();
+
+    odometry.prepareFrame(odf);
+    bool isComputed = odometry.compute(odf, odf, calcRt);
+    ASSERT_TRUE(isComputed);
+    double diff = cv::norm(calcRt, Mat::eye(4, 4, CV_64FC1));
+    ASSERT_LE(diff, idError) << "Incorrect transformation between the same frame (not the identity matrix)" << std::endl;
+}
+
+void OdometryTest::run()
+{
+    Mat K = getCameraMatrix();
+
+    Mat image, depth;
+    readData(image, depth);
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf(depth, image);
+    Mat calcRt;
+
+    // 1. Try to find Rt between the same frame (try masks also).
+    Mat mask(image.size(), CV_8UC1, Scalar(255));
+
+    odometry.prepareFrame(odf);
+    bool isComputed = odometry.compute(odf, odf, calcRt);
+
+    ASSERT_TRUE(isComputed) << "Can not find Rt between the same frame" << std::endl;
+    double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1));
+    ASSERT_LE(ndiff, idError) << "Incorrect transformation between the same frame (not the identity matrix)" << std::endl;
+
+    // 2. Generate random rigid body motion in some ranges several times (iterCount).
+    // On each iteration an input frame is warped using generated transformation.
+    // Odometry is run on the following pair: the original frame and the warped one.
+    // Comparing a computed transformation with an applied one we compute 2 errors:
+    // better_1time_count - count of poses which error is less than ground truth pose,
+    // better_5times_count - count of poses which error is 5 times less than ground truth pose.
+    int iterCount = 100;
+    int better_1time_count = 0;
+    int better_5times_count = 0;
+    for (int iter = 0; iter < iterCount; iter++)
+    {
+        Mat rvec, tvec;
+        generateRandomTransformation(rvec, tvec);
+        Affine3d rt(rvec, tvec);
+
+        Mat warpedImage, warpedDepth;
+
+        warpFrame(depth, image, noArray(), rt.matrix, K, warpedDepth, warpedImage);
+        dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
+
+        OdometryFrame odfSrc(depth, image);
+        OdometryFrame odfDst(warpedDepth, warpedImage);
+
+        odometry.prepareFrames(odfSrc, odfDst);
+        isComputed = odometry.compute(odfSrc, odfDst, calcRt);
+
+        if (!isComputed)
+        {
+            CV_LOG_INFO(NULL, "Iter " << iter << "; Odometry compute returned false");
+            continue;
+        }
+        Mat calcR = calcRt(Rect(0, 0, 3, 3)), calcRvec;
+        cv::Rodrigues(calcR, calcRvec);
+        calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows);
+        Mat calcTvec = calcRt(Rect(3,0,1,3));
+
+        if (cvtest::debugLevel >= 10)
+        {
+            imshow("image", image);
+            imshow("warpedImage", warpedImage);
+            Mat resultImage, resultDepth;
+
+            warpFrame(depth, image, noArray(), calcRt, K, resultDepth, resultImage);
+            imshow("resultImage", resultImage);
+            waitKey(100);
+        }
+
+        // compare rotation
+        double possibleError = algtype == OdometryAlgoType::COMMON ? 0.02f : 0.02f;
+
+        Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec));
+        Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec));
+        Affine3f src_inv = src.inv();
+        Affine3f diff = res * src_inv;
+        double rdiffnorm = cv::norm(diff.rvec());
+        double tdiffnorm = cv::norm(diff.translation());
+
+        if (rdiffnorm < possibleError && tdiffnorm < possibleError)
+            better_1time_count++;
+        if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError)
+            better_5times_count++;
+
+        CV_LOG_INFO(NULL, "Iter " << iter);
+        CV_LOG_INFO(NULL, "rdiff: " << Vec3f(diff.rvec()) << "; rdiffnorm: " << rdiffnorm);
+        CV_LOG_INFO(NULL, "tdiff: " << Vec3f(diff.translation()) << "; tdiffnorm: " << tdiffnorm);
+
+        CV_LOG_INFO(NULL, "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count);
+    }
+
+    if(static_cast<double>(better_1time_count) < maxError1 * static_cast<double>(iterCount))
+    {
+        FAIL() << "Incorrect count of accurate poses [1st case]: "
+            << static_cast<double>(better_1time_count) << " / "
+            << maxError1 * static_cast<double>(iterCount) << std::endl;
+    }
+
+    if(static_cast<double>(better_5times_count) < maxError5 * static_cast<double>(iterCount))
+    {
+        FAIL() << "Incorrect count of accurate poses [2nd case]: "
+            << static_cast<double>(better_5times_count) << " / "
+            << maxError5 * static_cast<double>(iterCount) << std::endl;
+    }
+}
+
+void OdometryTest::prepareFrameCheck()
+{
+    Mat K = getCameraMatrix();
+
+    Mat gtImage, gtDepth;
+    readData(gtImage, gtDepth);
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf(gtDepth, gtImage);
+
+    odometry.prepareFrame(odf);
+
+    std::vector<int> iters;
+    ods.getIterCounts(iters);
+    size_t nlevels = iters.size();
+
+    Mat points, mask, depth, gray, rgb, scaled;
+
+    odf.getMask(mask);
+    int masknz = countNonZero(mask);
+    ASSERT_GT(masknz, 0);
+
+    odf.getDepth(depth);
+    Mat patchedDepth = depth.clone();
+    patchNaNs(patchedDepth, 0);
+    int depthnz = countNonZero(patchedDepth);
+    double depthNorm = cv::norm(depth, gtDepth, NORM_INF, mask);
+    ASSERT_LE(depthNorm, 0.0);
+
+    Mat gtGray;
+    if (otype == OdometryType::RGB || otype == OdometryType::RGB_DEPTH)
+    {
+        odf.getGrayImage(gray);
+        odf.getImage(rgb);
+        double rgbNorm = cv::norm(rgb, gtImage);
+        ASSERT_LE(rgbNorm, 0.0);
+
+        if (gtImage.channels() == 3)
+            cvtColor(gtImage, gtGray, COLOR_BGR2GRAY);
+        else
+            gtGray = gtImage;
+        gtGray.convertTo(gtGray, CV_8U);
+        double grayNorm = cv::norm(gray, gtGray);
+        ASSERT_LE(grayNorm, 0.0);
+    }
+
+    odf.getProcessedDepth(scaled);
+    int scalednz = countNonZero(scaled);
+    EXPECT_EQ(scalednz, depthnz);
+
+    std::vector<Mat> gtPyrDepth, gtPyrMask;
+    //TODO: this depth calculation would become incorrect when we implement bilateral filtering, fixit
+    buildPyramid(gtDepth, gtPyrDepth, (int)nlevels - 1);
+    for (const auto& gd : gtPyrDepth)
+    {
+        Mat pm = (gd > ods.getMinDepth()) & (gd < ods.getMaxDepth());
+        gtPyrMask.push_back(pm);
+    }
+
+    size_t npyr = odf.getPyramidLevels();
+    ASSERT_EQ(npyr, nlevels);
+    Matx33f levelK = K;
+    for (size_t i = 0; i < nlevels; i++)
+    {
+        Mat depthi, cloudi, maski;
+
+        odf.getPyramidAt(maski, OdometryFramePyramidType::PYR_MASK, i);
+        ASSERT_FALSE(maski.empty());
+        double mnorm = cv::norm(maski, gtPyrMask[i]);
+        EXPECT_LE(mnorm, 16 * 255.0) << "Mask diff is too big at pyr level " << i;
+
+        odf.getPyramidAt(depthi, OdometryFramePyramidType::PYR_DEPTH, i);
+        ASSERT_FALSE(depthi.empty());
+        double dnorm = cv::norm(depthi, gtPyrDepth[i], NORM_INF, maski);
+        EXPECT_LE(dnorm, 8.e-7) << "Depth diff norm is too big at pyr level " << i;
+
+        odf.getPyramidAt(cloudi, OdometryFramePyramidType::PYR_CLOUD, i);
+        ASSERT_FALSE(cloudi.empty());
+        Mat gtCloud;
+        depthTo3d(depthi, levelK, gtCloud);
+        double cnorm = cv::norm(cloudi, gtCloud, NORM_INF, maski);
+        EXPECT_LE(cnorm, 0.0) << "Cloud diff norm is too big at pyr level " << i;
+        // downscale camera matrix for next pyramid level
+        levelK = 0.5f * levelK;
+        levelK(2, 2) = 1.f;
+    }
+
+    if (otype == OdometryType::RGB || otype == OdometryType::RGB_DEPTH)
+    {
+        std::vector<Mat> gtPyrImage;
+        buildPyramid(gtGray, gtPyrImage, (int)nlevels - 1);
+
+        for (size_t i = 0; i < nlevels; i++)
+        {
+            Mat rgbi, texi, dixi, diyi, maski;
+            odf.getPyramidAt(maski, OdometryFramePyramidType::PYR_MASK, i);
+            odf.getPyramidAt(rgbi, OdometryFramePyramidType::PYR_IMAGE, i);
+            ASSERT_FALSE(rgbi.empty());
+            double rnorm = cv::norm(rgbi, gtPyrImage[i], NORM_INF);
+            EXPECT_LE(rnorm, 1.0)  << "RGB diff is too big at pyr level " << i;
+            odf.getPyramidAt(texi, OdometryFramePyramidType::PYR_TEXMASK, i);
+            ASSERT_FALSE(texi.empty());
+            int tnz = countNonZero(texi);
+            EXPECT_GE(tnz, 1000) << "Texture mask has too few valid pixels at pyr level " << i;
+            Mat gtDixi, gtDiyi;
+            Sobel(rgbi, gtDixi, CV_16S, 1, 0, ods.getSobelSize());
+            odf.getPyramidAt(dixi, OdometryFramePyramidType::PYR_DIX, i);
+            ASSERT_FALSE(dixi.empty());
+            double dixnorm = cv::norm(dixi, gtDixi, NORM_INF, maski);
+            EXPECT_LE(dixnorm, 0) << "dI/dx diff is too big at pyr level " << i;
+            Sobel(rgbi, gtDiyi, CV_16S, 0, 1, ods.getSobelSize());
+            odf.getPyramidAt(diyi, OdometryFramePyramidType::PYR_DIY, i);
+            ASSERT_FALSE(diyi.empty());
+            double diynorm = cv::norm(diyi, gtDiyi, NORM_INF, maski);
+            EXPECT_LE(diynorm, 0) << "dI/dy diff is too big at pyr level " << i;
+        }
+    }
+
+    if (otype == OdometryType::DEPTH || otype == OdometryType::RGB_DEPTH)
+    {
+        Ptr<RgbdNormals> normalComputer = odometry.getNormalsComputer();
+        ASSERT_FALSE(normalComputer.empty());
+        Mat normals;
+        odf.getNormals(normals);
+        std::vector<Mat> gtPyrNormals;
+        buildPyramid(normals, gtPyrNormals, (int)nlevels - 1);
+        for (size_t i = 0; i < nlevels; i++)
+        {
+            Mat gtNormal = gtPyrNormals[i];
+            CV_Assert(gtNormal.type() == CV_32FC4);
+            for (int y = 0; y < gtNormal.rows; y++)
+            {
+                Vec4f *normals_row = gtNormal.ptr<Vec4f>(y);
+                for (int x = 0; x < gtNormal.cols; x++)
+                {
+                    Vec4f n4 = normals_row[x];
+                    Point3f n(n4[0], n4[1], n4[2]);
+                    double nrm = cv::norm(n);
+                    n *= 1.f / nrm;
+                    normals_row[x] = Vec4f(n.x, n.y, n.z, 0);
+                }
+            }
+
+            Mat normmaski;
+            odf.getPyramidAt(normmaski, OdometryFramePyramidType::PYR_NORMMASK, i);
+            ASSERT_FALSE(normmaski.empty());
+            int nnm = countNonZero(normmaski);
+            EXPECT_GE(nnm, 1000) << "Normals mask has too few valid pixels at pyr level " << i;
+
+            Mat ptsi;
+            odf.getPyramidAt(ptsi, OdometryFramePyramidType::PYR_CLOUD, i);
+
+            Mat normi;
+            odf.getPyramidAt(normi, OdometryFramePyramidType::PYR_NORM, i);
+            ASSERT_FALSE(normi.empty());
+            double nnorm = cv::norm(normi, gtNormal, NORM_INF, normmaski);
+            EXPECT_LE(nnorm, 3.3e-7) << "Normals diff is too big at pyr level " << i;
+
+            if (i == 0)
+            {
+                double pnnorm = cv::norm(normals, normi, NORM_INF, normmaski);
+                EXPECT_GE(pnnorm, 0);
+            }
+        }
+    }
+}
+
+
+void OdometryTest::processedDepthCheck()
+{
+    Mat K = getCameraMatrix();
+
+    Mat gtImage, gtDepth;
+    readData(gtImage, gtDepth);
+
+    gtDepth *= 5000.0;
+
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf(gtDepth, gtImage);
+
+    odometry.prepareFrame(odf);
+
+    Mat scaled;
+    odf.getProcessedDepth(scaled);
+
+    //TODO: remove this check when depth rescaling is removed
+    double pmax;
+    cv::minMaxLoc(scaled, nullptr, &pmax);
+    EXPECT_LT(pmax, 10.0);
+}
+
+/****************************************************************************************\
+*                                Tests registrations                                     *
+\****************************************************************************************/
+
+TEST(RGBD_Odometry_Rgb, algorithmic)
+{
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.run();
+}
+
+TEST(RGBD_Odometry_ICP, algorithmic)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.run();
+}
+
+TEST(RGBD_Odometry_RgbdICP, algorithmic)
+{
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.run();
+}
+
+TEST(RGBD_Odometry_FastICP, algorithmic)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.87, 1.84e-5);
+    test.run();
+}
+
+
+TEST(RGBD_Odometry_Rgb, UMats)
+{
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.checkUMats();
+}
+
+TEST(RGBD_Odometry_ICP, UMats)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.checkUMats();
+}
+
+TEST(RGBD_Odometry_RgbdICP, UMats)
+{
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.checkUMats();
+}
+
+TEST(RGBD_Odometry_FastICP, UMats)
+{
+    // OpenCL version has slightly less accuracy than CPU version
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, 1.84e-5);
+    test.checkUMats();
+}
+
+
+TEST(RGBD_Odometry_Rgb, prepareFrame)
+{
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.prepareFrameCheck();
+}
+
+TEST(RGBD_Odometry_ICP, prepareFrame)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.prepareFrameCheck();
+}
+
+TEST(RGBD_Odometry_RgbdICP, prepareFrame)
+{
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.prepareFrameCheck();
+}
+
+TEST(RGBD_Odometry_FastICP, prepareFrame)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, FLT_EPSILON);
+    test.prepareFrameCheck();
+}
+
+
+TEST(RGBD_Odometry_Rgb, processedDepth)
+{
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.processedDepthCheck();
+}
+
+TEST(RGBD_Odometry_ICP, processedDepth)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.processedDepthCheck();
+}
+
+TEST(RGBD_Odometry_RgbdICP, processedDepth)
+{
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.processedDepthCheck();
+}
+
+TEST(RGBD_Odometry_FastICP, processedDepth)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, FLT_EPSILON);
+    test.processedDepthCheck();
+}
+
+
+struct WarpFrameTest
+{
+    WarpFrameTest() :
+     srcDepth(), srcRgb(), srcMask(),
+     dstDepth(), dstRgb(), dstMask(),
+     warpedDepth(), warpedRgb(), warpedMask()
+      {}
+
+    void run(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform, int depthType, int imageType);
+
+    Mat srcDepth, srcRgb, srcMask;
+    Mat dstDepth, dstRgb, dstMask;
+    Mat warpedDepth, warpedRgb, warpedMask;
+};
+
+void WarpFrameTest::run(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform, int depthType, int rgbType)
+{
+    std::string dataPath = cvtest::TS::ptr()->get_data_path();
+    std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png";
+    std::string srcRgbFilename   = dataPath + "/cv/rgbd/rgb.png";
+    // The depth was generated using the script at testdata/cv/rgbd/warped_depth_generator/warp_test.py
+    std::string warpedDepthFilename = dataPath + "/cv/rgbd/warpedDepth.png";
+    std::string warpedRgbFilename   = dataPath + "/cv/rgbd/warpedRgb.png";
+
+    srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED);
+    ASSERT_FALSE(srcDepth.empty())  << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl;
+
+    if (identityTransform)
+    {
+        warpedDepth = srcDepth;
+    }
+    else
+    {
+        warpedDepth = imread(warpedDepthFilename, IMREAD_UNCHANGED);
+        ASSERT_FALSE(warpedDepth.empty()) << "Depth " << warpedDepthFilename.c_str() << "can not be read" << std::endl;
+    }
+
+    ASSERT_TRUE(srcDepth.type() == CV_16UC1);
+    ASSERT_TRUE(warpedDepth.type() == CV_16UC1);
+
+    Mat epsSrc = srcDepth > 0, epsWarped = warpedDepth > 0;
+
+    const double depthFactor = 5000.0;
+    // scale float types only
+    double depthScaleCoeff = scaleDown ? ( depthType == CV_16U ? 1.          : 1./depthFactor ) :    1.;
+    double transScaleCoeff = scaleDown ? ( depthType == CV_16U ? depthFactor : 1.             ) : depthFactor;
+
+    Mat srcDepthCvt, warpedDepthCvt;
+    srcDepth.convertTo(srcDepthCvt, depthType, depthScaleCoeff);
+    srcDepth = srcDepthCvt;
+    warpedDepth.convertTo(warpedDepthCvt, depthType, depthScaleCoeff);
+    warpedDepth = warpedDepthCvt;
+
+    Scalar badVal;
+    switch (depthType)
+    {
+    case CV_16U:
+        badVal = 0;
+        break;
+    case CV_32F:
+        badVal = std::numeric_limits<float>::quiet_NaN();
+        break;
+    case CV_64F:
+        badVal = std::numeric_limits<double>::quiet_NaN();
+        break;
+    default:
+        CV_Error(Error::StsBadArg, "Unsupported depth data type");
+    }
+
+    srcDepth.setTo(badVal, ~epsSrc);
+    warpedDepth.setTo(badVal, ~epsWarped);
+
+    if (checkMask)
+    {
+        srcMask = epsSrc; warpedMask = epsWarped;
+    }
+    else
+    {
+        srcMask = Mat(); warpedMask = Mat();
+    }
+
+    if (needRgb)
+    {
+        srcRgb = imread(srcRgbFilename, rgbType == CV_8UC1 ? IMREAD_GRAYSCALE : IMREAD_COLOR);
+        ASSERT_FALSE(srcRgb.empty()) << "Image " << srcRgbFilename.c_str() << "can not be read" << std::endl;
+
+        if (identityTransform)
+        {
+            srcRgb.copyTo(warpedRgb, epsSrc);
+        }
+        else
+        {
+            warpedRgb = imread(warpedRgbFilename, rgbType == CV_8UC1 ? IMREAD_GRAYSCALE : IMREAD_COLOR);
+            ASSERT_FALSE (warpedRgb.empty()) << "Image " << warpedRgbFilename.c_str() << "can not be read" << std::endl;
+        }
+
+        if (rgbType == CV_8UC4)
+        {
+            Mat newSrcRgb, newWarpedRgb;
+            cvtColor(srcRgb, newSrcRgb, COLOR_RGB2RGBA);
+            srcRgb = newSrcRgb;
+            // let's keep alpha channel
+            std::vector<Mat> warpedRgbChannels;
+            split(warpedRgb, warpedRgbChannels);
+            warpedRgbChannels.push_back(epsWarped);
+            merge(warpedRgbChannels, newWarpedRgb);
+            warpedRgb = newWarpedRgb;
+        }
+
+        ASSERT_TRUE(srcRgb.type() == rgbType);
+        ASSERT_TRUE(warpedRgb.type() == rgbType);
+    }
+    else
+    {
+        srcRgb = Mat(); warpedRgb = Mat();
+    }
+
+    // test data used to generate warped depth and rgb
+    // the script used to generate is in opencv_extra repo
+    // at testdata/cv/rgbd/warped_depth_generator/warp_test.py
+    double fx = 525.0, fy = 525.0,
+           cx = 319.5, cy = 239.5;
+    Matx33d K(fx,  0, cx,
+               0, fy, cy,
+               0,  0,  1);
+    cv::Affine3d rt;
+    cv::Vec3d tr(-0.04, 0.05, 0.6);
+    rt = identityTransform ? cv::Affine3d() : cv::Affine3d(cv::Vec3d(0.1, 0.2, 0.3), tr * transScaleCoeff);
+
+    warpFrame(srcDepth, srcRgb, srcMask, rt.matrix, K, dstDepth, dstRgb, dstMask);
+}
+
+typedef std::pair<int, int> WarpFrameInputTypes;
+typedef testing::TestWithParam<WarpFrameInputTypes> WarpFrameInputs;
+
+TEST_P(WarpFrameInputs, checkTypes)
+{
+    const double shortl2diff = 233.983;
+    const double shortlidiff = 1;
+    const double floatl2diff = 0.038209;
+    const double floatlidiff = 0.00020004;
+
+    int depthType = GetParam().first;
+    int rgbType   = GetParam().second;
+
+    WarpFrameTest w;
+    // scale down does not happen on CV_16U
+    // to avoid integer overflow
+    w.run(/* needRgb */ true, /* scaleDown*/ true,
+          /* checkMask */ true, /* identityTransform */ false, depthType, rgbType);
+
+    double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
+    double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
+
+    EXPECT_EQ(0, maskDiff);
+    EXPECT_EQ(0, rgbDiff);
+
+    double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask);
+    double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask);
+
+    double l2threshold = depthType == CV_16U ? shortl2diff : floatl2diff;
+    double lithreshold = depthType == CV_16U ? shortlidiff : floatlidiff;
+
+    EXPECT_LE(l2diff, l2threshold);
+    EXPECT_LE(lidiff, lithreshold);
+}
+
+INSTANTIATE_TEST_CASE_P(RGBD_Odometry, WarpFrameInputs, ::testing::Values(
+                        WarpFrameInputTypes { CV_16U, CV_8UC3 },
+                        WarpFrameInputTypes { CV_32F, CV_8UC3 },
+                        WarpFrameInputTypes { CV_64F, CV_8UC3 },
+                        WarpFrameInputTypes { CV_32F, CV_8UC1 },
+                        WarpFrameInputTypes { CV_32F, CV_8UC4 }));
+
+
+TEST(RGBD_Odometry_WarpFrame, identity)
+{
+    WarpFrameTest w;
+    w.run(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ true, CV_32F, CV_8UC3);
+
+    double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
+    double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
+
+    ASSERT_EQ(0, rgbDiff);
+    ASSERT_EQ(0, maskDiff);
+
+    double depthDiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.dstMask);
+
+    ASSERT_LE(depthDiff, DBL_EPSILON);
+}
+
+TEST(RGBD_Odometry_WarpFrame, noRgb)
+{
+    WarpFrameTest w;
+    w.run(/* needRgb */ false, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ false, CV_32F, CV_8UC3);
+
+    double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
+    ASSERT_EQ(0, maskDiff);
+
+    double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask);
+    double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask);
+
+    ASSERT_LE(l2diff, 0.038209);
+    ASSERT_LE(lidiff, 0.00020004);
+}
+
+TEST(RGBD_Odometry_WarpFrame, nansAreMasked)
+{
+    WarpFrameTest w;
+    w.run(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ false, /* identityTransform */ false, CV_32F, CV_8UC3);
+
+    double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
+
+    ASSERT_EQ(0, rgbDiff);
+
+    Mat goodVals;
+    finiteMask(w.warpedDepth, goodVals);
+
+    double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, goodVals);
+    double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, goodVals);
+
+    ASSERT_LE(l2diff, 0.038209);
+    ASSERT_LE(lidiff, 0.00020004);
+}
+
+TEST(RGBD_Odometry_WarpFrame, bigScale)
+{
+    WarpFrameTest w;
+    w.run(/* needRgb */ true, /* scaleDown*/ false, /* checkMask */ true, /* identityTransform */ false, CV_32F, CV_8UC3);
+
+    double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
+    double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
+
+    ASSERT_EQ(0, maskDiff);
+    ASSERT_EQ(0, rgbDiff);
+
+    double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask);
+    double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask);
+
+    ASSERT_LE(l2diff, 191.026565);
+    ASSERT_LE(lidiff, 0.99951172);
+}
+
+TEST(RGBD_DepthTo3D, mask)
+{
+    std::string dataPath = cvtest::TS::ptr()->get_data_path();
+    std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png";
+
+    Mat srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED);
+    ASSERT_FALSE(srcDepth.empty())  << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl;
+    ASSERT_TRUE(srcDepth.type() == CV_16UC1);
+
+    Mat srcMask = srcDepth > 0;
+
+    // test data used to generate warped depth and rgb
+    // the script used to generate is in opencv_extra repo
+    // at testdata/cv/rgbd/warped_depth_generator/warp_test.py
+    double fx = 525.0, fy = 525.0,
+           cx = 319.5, cy = 239.5;
+    Matx33d intr(fx,  0, cx,
+                  0, fy, cy,
+                  0,  0,  1);
+
+    Mat srcCloud;
+    depthTo3d(srcDepth, intr, srcCloud, srcMask);
+    size_t npts = countNonZero(srcMask);
+
+    ASSERT_EQ(npts, srcCloud.total());
+}
+
+}} // namespace

modules/ptcloud/test/test_pointcloud_io.cpp

@@ -0,0 +1,299 @@
+// 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.
+
+#include <opencv2/core.hpp>
+#include <vector>
+#include <cstdio>
+
+#include "test_precomp.hpp"
+#include "opencv2/ts.hpp"
+
+namespace opencv_test { namespace {
+
+struct OriginalObjGoldValues
+{
+    OriginalObjGoldValues()
+    {
+        std::array<float, 6> vals = { -5.93915f, -0.13257f, 2.55837f, 1.86743f, -1.16339f, 0.399941f };
+        points =
+        {
+            { vals[0], vals[1], vals[2] },
+            { vals[0], vals[3], vals[2] },
+            { vals[0], vals[1], vals[4] },
+            { vals[0], vals[3], vals[4] },
+            { vals[5], vals[1], vals[2] },
+            { vals[5], vals[3], vals[2] },
+            { vals[5], vals[1], vals[4] },
+            { vals[5], vals[3], vals[4] },
+        };
+
+        normals =
+        {
+            {-1.0f,  0.0f,  0.0f},
+            { 0.0f,  0.0f, -1.0f},
+            { 1.0f,  0.0f,  0.0f},
+            { 0.0f,  0.0f,  1.0f},
+            { 0.0f, -1.0f,  0.0f},
+            { 0.0f,  1.0f,  0.0f}
+        };
+
+        rgb =
+        {
+            {0.0756f, 0.5651f, 0.5829f},
+            {0.8596f, 0.1105f, 0.8455f},
+            {0.8534f, 0.6143f, 0.3950f},
+            {0.0438f, 0.6308f, 0.3065f},
+            {0.9716f, 0.7170f, 0.8378f},
+            {0.2472f, 0.7701f, 0.0234f},
+            {0.6472f, 0.7467f, 0.5981f},
+            {0.3502f, 0.7954f, 0.0443f}
+        };
+
+        std::vector<std::pair<int, int>> tcvals =
+        {
+            { 3, 0 },
+            { 5, 0 },
+            { 5, 2 },
+            { 3, 2 },
+            { 5, 4 },
+            { 3, 4 },
+            { 5, 6 },
+            { 3, 6 },
+            { 5, 8 },
+            { 3, 8 },
+            { 1, 4 },
+            { 1, 6 },
+            { 7, 4 },
+            { 7, 6 },
+        };
+
+        for (const auto& p : tcvals)
+        {
+            texCoords.push_back({p.first * 0.125f, p.second * 0.125f});
+        }
+
+        // mesh data is duplicated for each face
+        std::vector<std::array<int, 9>> fileIndices =
+        {
+            { 1,  1,  1, /**/ 2,  2,  1, /**/ 4,  3,  1 },
+            { 3,  4,  2, /**/ 4,  3,  2, /**/ 8,  5,  2 },
+            { 7,  6,  3, /**/ 8,  5,  3, /**/ 6,  7,  3 },
+            { 5,  8,  4, /**/ 6,  7,  4, /**/ 2,  9,  4 },
+            { 3, 11,  5, /**/ 7,  6,  5, /**/ 5,  8,  5 },
+            { 8,  5,  6, /**/ 4, 13,  6, /**/ 2, 14,  6 },
+        };
+
+        for (const auto& fi : fileIndices)
+        {
+            pointsMesh.push_back(points.at(fi[0] - 1));
+            pointsMesh.push_back(points.at(fi[3] - 1));
+            pointsMesh.push_back(points.at(fi[6] - 1));
+            rgbMesh.push_back(rgb.at(fi[0] - 1));
+            rgbMesh.push_back(rgb.at(fi[3] - 1));
+            rgbMesh.push_back(rgb.at(fi[6] - 1));
+
+            texCoordsMesh.push_back(texCoords.at(fi[1] - 1));
+            texCoordsMesh.push_back(texCoords.at(fi[4] - 1));
+            texCoordsMesh.push_back(texCoords.at(fi[7] - 1));
+
+            normalsMesh.push_back(normals.at(fi[2] - 1));
+            normalsMesh.push_back(normals.at(fi[5] - 1));
+            normalsMesh.push_back(normals.at(fi[8] - 1));
+        }
+
+        indices =
+        {
+            { 0,  1,  2},
+            { 3,  4,  5},
+            { 6,  7,  8},
+            { 9, 10, 11},
+            {12, 13, 14},
+            {15, 16, 17},
+        };
+    }
+
+    std::vector<Point3f> points, pointsMesh, normals, normalsMesh, rgb, rgbMesh;
+    std::vector<Point2f> texCoords, texCoordsMesh;
+    std::vector<std::vector<int32_t>> indices;
+};
+
+OriginalObjGoldValues origGold;
+
+TEST(PointCloud, LoadPointCloudObj)
+{
+    std::vector<cv::Point3f> points, normals, rgb;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    cv::loadPointCloud(folder + "pointcloudio/orig.obj", points, normals, rgb);
+
+    EXPECT_EQ(origGold.points, points);
+    EXPECT_EQ(origGold.rgb, rgb);
+    EXPECT_EQ(origGold.normals, normals);
+}
+
+TEST(PointCloud, LoadObjNoNormals)
+{
+    std::vector<cv::Point3f> points, normals;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    cv::loadPointCloud(folder + "pointcloudio/orig_no_norms.obj", points, normals);
+
+    EXPECT_EQ(origGold.points, points);
+    EXPECT_TRUE(normals.empty());
+}
+
+TEST(PointCloud, SaveObj)
+{
+    std::vector<cv::Point3f> points_gold, normals_gold, rgb_gold;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    auto new_path = tempfile("new.obj");
+
+    cv::loadPointCloud(folder + "pointcloudio/orig.obj", points_gold, normals_gold, rgb_gold);
+    cv::savePointCloud(new_path, points_gold, normals_gold, rgb_gold);
+
+    std::vector<cv::Point3f> points, normals, rgb;
+
+    cv::loadPointCloud(new_path, points, normals, rgb);
+
+    EXPECT_EQ(normals, normals_gold);
+    EXPECT_EQ(points, points_gold);
+    EXPECT_EQ(rgb, rgb_gold);
+    std::remove(new_path.c_str());
+}
+
+TEST(PointCloud, LoadSavePly)
+{
+    std::vector<cv::Point3f> points, normals, rgb;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    std::string new_path = tempfile("new.ply");
+
+    cv::loadPointCloud(folder + "pointcloudio/orig.ply", points, normals, rgb);
+    cv::savePointCloud(new_path, points, normals, rgb);
+
+    std::vector<cv::Point3f> points_gold, normals_gold, rgb_gold;
+
+    cv::loadPointCloud(new_path, points_gold, normals_gold, rgb_gold);
+
+    EXPECT_EQ(normals_gold, normals);
+    EXPECT_EQ(points_gold, points);
+    EXPECT_EQ(rgb_gold, rgb);
+    std::remove(new_path.c_str());
+}
+
+TEST(PointCloud, LoadSaveMeshObj)
+{
+    std::vector<cv::Point3f> points, normals, colors;
+    std::vector<cv::Point2f> texCoords;
+    std::vector<std::vector<int32_t>> indices;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    std::string new_path = tempfile("new_mesh.obj");
+
+    cv::loadMesh(folder + "pointcloudio/orig.obj", points, indices, normals, colors, texCoords);
+    EXPECT_EQ(origGold.pointsMesh, points);
+    EXPECT_EQ(origGold.indices, indices);
+    EXPECT_EQ(origGold.normalsMesh, normals);
+    EXPECT_EQ(origGold.rgbMesh, colors);
+    EXPECT_EQ(origGold.texCoordsMesh, texCoords);
+    cv::saveMesh(new_path, points, indices, normals, colors, texCoords);
+
+    std::vector<cv::Point3f> points_gold, normals_gold, colors_gold;
+    std::vector<cv::Point2f> texCoords_gold;
+    std::vector<std::vector<int32_t>> indices_gold;
+
+    cv::loadMesh(new_path, points_gold, indices_gold, normals_gold, colors_gold, texCoords_gold);
+    EXPECT_FALSE(points_gold.empty());
+    EXPECT_FALSE(indices_gold.empty());
+    EXPECT_FALSE(normals_gold.empty());
+    EXPECT_FALSE(colors_gold.empty());
+    EXPECT_FALSE(texCoords_gold.empty());
+
+    EXPECT_EQ(normals_gold, normals);
+    EXPECT_EQ(points_gold, points);
+    EXPECT_EQ(indices_gold, indices);
+    EXPECT_TRUE(!indices.empty());
+    std::remove(new_path.c_str());
+}
+
+typedef std::string PlyTestParamsType;
+typedef testing::TestWithParam<PlyTestParamsType> PlyTest;
+
+TEST_P(PlyTest, LoadSaveMesh)
+{
+    std::string fname = GetParam();
+
+    std::vector<cv::Point3f> points_gold, normals_gold, colors_gold;
+    std::vector<cv::Vec3i> indices_gold;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    std::string new_path = tempfile("new_mesh.ply");
+
+    cv::loadMesh(folder + fname, points_gold, indices_gold, normals_gold, colors_gold);
+    size_t truePts, trueFaces;
+    if (fname.find("/dragon.ply") != fname.npos)
+    {
+        truePts = 50000; trueFaces = 100000;
+    }
+    else
+    {
+        truePts = 8; trueFaces = 12;
+    }
+    EXPECT_EQ(points_gold.size(), truePts);
+    EXPECT_EQ(indices_gold.size(), trueFaces);
+
+    cv::saveMesh(new_path, points_gold, indices_gold, normals_gold, colors_gold);
+
+    std::vector<cv::Point3f> points, normals, colors;
+    std::vector<cv::Vec3i> indices;
+    cv::loadMesh(new_path, points, indices, normals, colors);
+
+    if (!normals.empty())
+    {
+        EXPECT_LE(cv::norm(normals_gold, normals, NORM_INF), 0);
+    }
+    EXPECT_LE(cv::norm(points_gold, points, NORM_INF), 0);
+    EXPECT_LE(cv::norm(colors_gold, colors, NORM_INF), 0);
+    EXPECT_EQ(indices_gold, indices);
+    std::remove(new_path.c_str());
+}
+
+
+INSTANTIATE_TEST_CASE_P(PointCloud, PlyTest,
+    ::testing::Values("pointcloudio/orig.ply", "pointcloudio/orig_ascii_fidx.ply", "pointcloudio/orig_bin_fidx.ply",
+                      "pointcloudio/orig_ascii_vidx.ply", "pointcloudio/orig_bin.ply", "viz/dragon.ply"));
+
+TEST(PointCloud, NonexistentFile)
+{
+    std::vector<cv::Point3f> points;
+    std::vector<cv::Point3f> normals;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    cv::loadPointCloud(folder + "pointcloudio/fake.obj", points, normals);
+    EXPECT_TRUE(points.empty());
+    EXPECT_TRUE(normals.empty());
+}
+
+TEST(PointCloud, LoadBadExtension)
+{
+    std::vector<cv::Point3f> points;
+    std::vector<cv::Point3f> normals;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    cv::loadPointCloud(folder + "pointcloudio/fake.fake", points, normals);
+    EXPECT_TRUE(points.empty());
+    EXPECT_TRUE(normals.empty());
+}
+
+TEST(PointCloud, SaveBadExtension)
+{
+    std::vector<cv::Point3f> points;
+    std::vector<cv::Point3f> normals;
+
+    auto folder = cvtest::TS::ptr()->get_data_path();
+    cv::savePointCloud(folder + "pointcloudio/fake.fake", points, normals);
+}
+
+}} /* namespace opencv_test */

modules/ptcloud/test/test_pose_graph.cpp

@@ -0,0 +1,562 @@
+// 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
+
+#include "test_precomp.hpp"
+#include <opencv2/ptcloud/detail/pose_graph.hpp>
+
+#include <opencv2/core/dualquaternion.hpp>
+
+namespace opencv_test { namespace {
+
+using namespace cv;
+
+#ifdef HAVE_EIGEN
+
+static Affine3d readAffine(std::istream& input)
+{
+    Vec3d p;
+    Vec4d q;
+    input >> p[0] >> p[1] >> p[2];
+    input >> q[1] >> q[2] >> q[3] >> q[0];
+    // Normalize the quaternion to account for precision loss due to
+    // serialization.
+    return Affine3d(Quatd(q).toRotMat3x3(), p);
+};
+
+// Rewritten from Ceres pose graph demo: https://ceres-solver.org/
+static Ptr<detail::PoseGraph> readG2OFile(const std::string& g2oFileName)
+{
+    Ptr<detail::PoseGraph> pg = detail::PoseGraph::create();
+
+    // for debugging purposes
+    size_t minId = 0, maxId = 1 << 30;
+
+    std::ifstream infile(g2oFileName.c_str());
+    if (!infile)
+    {
+        CV_Error(cv::Error::StsError, "failed to open file");
+    }
+
+    while (infile.good())
+    {
+        std::string data_type;
+        // Read whether the type is a node or a constraint
+        infile >> data_type;
+        if (data_type == "VERTEX_SE3:QUAT")
+        {
+            size_t id;
+            infile >> id;
+            Affine3d pose = readAffine(infile);
+
+            if (id < minId || id >= maxId)
+                continue;
+
+            bool fixed = (id == minId);
+
+            // Ensure we don't have duplicate poses
+            if (pg->isNodeExist(id))
+            {
+                CV_LOG_INFO(NULL, "duplicated node, id=" << id);
+            }
+            pg->addNode(id, pose, fixed);
+        }
+        else if (data_type == "EDGE_SE3:QUAT")
+        {
+            size_t startId, endId;
+            infile >> startId >> endId;
+            Affine3d pose = readAffine(infile);
+
+            Matx66d info;
+            for (int i = 0; i < 6 && infile.good(); ++i)
+            {
+                for (int j = i; j < 6 && infile.good(); ++j)
+                {
+                    infile >> info(i, j);
+                    if (i != j)
+                    {
+                        info(j, i) = info(i, j);
+                    }
+                }
+            }
+
+            if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId))
+            {
+                pg->addEdge(startId, endId, pose, info);
+            }
+        }
+        else
+        {
+            CV_Error(cv::Error::StsError, "unknown tag");
+        }
+
+        // Clear any trailing whitespace from the line
+        infile >> std::ws;
+    }
+
+    return pg;
+}
+
+
+TEST(PoseGraph, sphereG2O)
+{
+    // Test takes 15+ sec in Release mode and 400+ sec in Debug mode
+    applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
+
+    // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/
+    // Connected paper:
+    // L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert.
+    // Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization.
+    // In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015.
+
+    std::string filename = cvtest::TS::ptr()->get_data_path() + "/cv/rgbd/sphere_bignoise_vertex3.g2o";
+
+    Ptr<detail::PoseGraph> pg = readG2OFile(filename);
+
+    // You may change logging level to view detailed optimization report
+    // For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO
+
+    // geoScale=1 is experimental, not guaranteed to work on other problems
+    // the rest are default params
+    pg->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
+                        .setMaxIterations(100)
+                        .setCheckRelEnergyChange(true)
+                        .setRelEnergyDeltaTolerance(1e-6)
+                        .setGeodesic(true));
+
+    auto r = pg->optimize();
+
+    EXPECT_TRUE(r.found);
+    EXPECT_LE(r.iters, 20); // should converge in 31 iterations
+
+    EXPECT_LE(r.energy, 1.47723e+06); // should converge to 1.47722e+06 or less
+
+    // Add the "--test_debug" to arguments to see resulting pose graph nodes positions
+    if (cvtest::debugLevel > 0)
+    {
+        // Write edge-only model of how nodes are located in space
+        std::string fname = "pgout.obj";
+        std::fstream of(fname, std::fstream::out);
+        std::vector<size_t> ids = pg->getNodesIds();
+        for (const size_t& id : ids)
+        {
+            Point3d d = pg->getNodePose(id).translation();
+            of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
+        }
+
+        size_t esz = pg->getNumEdges();
+        for (size_t i = 0; i < esz; i++)
+        {
+            size_t sid = pg->getEdgeStart(i), tid = pg->getEdgeEnd(i);
+            of << "l " << sid + 1 << " " << tid + 1 << std::endl;
+        }
+
+        of.close();
+    }
+}
+
+TEST(PoseGraphMST, optimization)
+{
+    applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
+
+    // The dataset was taken from here: https://lucacarlone.mit.edu/datasets/
+    // Connected paper:
+    // L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert.
+    // Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization.
+    // In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015.
+
+    std::string filename = cvtest::TS::ptr()->get_data_path() + "/cv/rgbd/sphere_bignoise_vertex3.g2o";
+
+    Ptr<detail::PoseGraph> pgOptimizerOnly = readG2OFile(filename);
+    Ptr<detail::PoseGraph> pgWithMSTAndOptimizer = readG2OFile(filename);
+    Ptr<detail::PoseGraph> init = readG2OFile(filename);
+
+    double lambda = 0.485;
+    pgWithMSTAndOptimizer->initializePosesWithMST(lambda);
+
+    // You may change logging level to view detailed optimization report
+    // For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO
+
+    // geoScale=1 is experimental, not guaranteed to work on other problems
+    // the rest are default params
+    pgOptimizerOnly->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
+                        .setMaxIterations(100)
+                        .setCheckRelEnergyChange(true)
+                        .setRelEnergyDeltaTolerance(1e-6)
+                        .setGeodesic(true));
+    pgWithMSTAndOptimizer->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
+                        .setMaxIterations(100)
+                        .setCheckRelEnergyChange(true)
+                        .setRelEnergyDeltaTolerance(1e-6)
+                        .setGeodesic(true));
+
+    auto r1 = pgWithMSTAndOptimizer->optimize();
+    auto r2 = pgOptimizerOnly->optimize();
+
+    EXPECT_TRUE(r1.found);
+    EXPECT_TRUE(r2.found);
+    EXPECT_LE(r2.energy, 1.47723e+06);
+    // Allow small tolerance due to optimization differences; final energy/iterations are effectively the same
+    EXPECT_LE(std::abs(r1.energy - r2.energy), 1e-2);
+    ASSERT_LE(std::abs(r1.iters - r2.iters), 1);
+
+    // Add the "--test_debug" to arguments to see resulting pose graph nodes positions
+    if (cvtest::debugLevel > 0)
+    {
+        // Note:
+        // A custom .obj writer is used here instead of cv::saveMesh because saveMesh expects faces
+        // (i.e., polygons with 3 or more vertices) and writes them using the "f i j k..." syntax in
+        // the .obj file. Since pose graphs consist of edges rather than polygonal faces, we represent
+        // them using line segments ("l i j").
+        // As saveMesh does not support writing "l" lines, it is not suitable in this context.
+
+        std::string fname;
+        std::fstream of;
+        std::vector<size_t> ids;
+        size_t esz;
+
+        // Write OBJ for MST-initialized pose graph with optimizer
+        fname = "pg_with_mst_and_optimizer.obj";
+        of.open(fname, std::fstream::out);
+        ids = pgWithMSTAndOptimizer->getNodesIds();
+        for (const size_t& id : ids)
+        {
+            Point3d d = pgWithMSTAndOptimizer->getNodePose(id).translation();
+            of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
+        }
+        esz = pgWithMSTAndOptimizer->getNumEdges();
+        for (size_t i = 0; i < esz; i++)
+        {
+            size_t sid = pgWithMSTAndOptimizer->getEdgeStart(i), tid = pgWithMSTAndOptimizer->getEdgeEnd(i);
+            of << "l " << sid + 1 << " " << tid + 1 << std::endl;
+        }
+        of.close();
+
+        // Write OBJ for optimizer-only pose graph
+        fname = "pg_optimizer_only.obj";
+        of.open(fname, std::fstream::out);
+        ids = pgOptimizerOnly->getNodesIds();
+        for (const size_t& id : ids)
+        {
+            Point3d d = pgOptimizerOnly->getNodePose(id).translation();
+            of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
+        }
+        esz = pgOptimizerOnly->getNumEdges();
+        for (size_t i = 0; i < esz; i++)
+        {
+            size_t sid = pgOptimizerOnly->getEdgeStart(i), tid = pgOptimizerOnly->getEdgeEnd(i);
+            of << "l " << sid + 1 << " " << tid + 1 << std::endl;
+        }
+        of.close();
+
+        // Write OBJ for initial pose graph
+        fname = "pg_init.obj";
+        of.open(fname, std::fstream::out);
+        ids = init->getNodesIds();
+        for (const size_t& id : ids)
+        {
+            Point3d d = init->getNodePose(id).translation();
+            of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
+        }
+        esz = init->getNumEdges();
+        for (size_t i = 0; i < esz; i++)
+        {
+            size_t sid = init->getEdgeStart(i), tid = init->getEdgeEnd(i);
+            of << "l " << sid + 1 << " " << tid + 1 << std::endl;
+        }
+        of.close();
+    }
+}
+
+// ------------------------------------------------------------------------------------------
+
+// Wireframe meshes for debugging visualization purposes
+struct Mesh
+{
+    std::vector<Point3f> pts;
+    std::vector<Vec2i> lines;
+
+    Mesh join(const Mesh& m2) const
+    {
+        Mesh mo;
+
+        size_t sz1 = this->pts.size();
+        std::copy(this->pts.begin(), this->pts.end(), std::back_inserter(mo.pts));
+        std::copy(m2.pts.begin(), m2.pts.end(), std::back_inserter(mo.pts));
+
+        std::copy(this->lines.begin(), this->lines.end(), std::back_inserter(mo.lines));
+        std::transform(m2.lines.begin(), m2.lines.end(), std::back_inserter(mo.lines),
+                       [sz1](Vec2i ab) { return Vec2i(ab[0] + (int)sz1, ab[1] + (int)sz1); });
+
+        return mo;
+    }
+
+    Mesh transform(Affine3f a, float scale = 1.f) const
+    {
+        Mesh out;
+        out.lines = this->lines;
+        for (Point3f p : this->pts)
+        {
+            out.pts.push_back(a * (p * scale));
+        }
+        return out;
+    }
+
+    // 0-2 - min, 3-5 - max
+    Vec6f getBoundingBox() const
+    {
+        float maxv = std::numeric_limits<float>::max();
+        Vec3f xmin(maxv, maxv, maxv), xmax(-maxv, -maxv, -maxv);
+        for (Point3f p : this->pts)
+        {
+            xmin[0] = min(p.x, xmin[0]); xmin[1] = min(p.y, xmin[1]); xmin[2] = min(p.z, xmin[2]);
+            xmax[0] = max(p.x, xmax[0]); xmax[1] = max(p.y, xmax[1]); xmax[2] = max(p.z, xmax[2]);
+        }
+        return Vec6f(xmin[0], xmin[1], xmin[2], xmax[0], xmax[1], xmax[2]);
+    }
+};
+
+
+Mesh seg7(int d)
+{
+    const std::vector<Point3f> pt = { {0, 0, 0}, {0, 1, 0},
+                                      {1, 0, 0}, {1, 1, 0},
+                                      {2, 0, 0}, {2, 1, 0} };
+
+    std::vector<Mesh> seg(7);
+    seg[0].pts = { pt[0], pt[1] };
+    seg[1].pts = { pt[1], pt[3] };
+    seg[2].pts = { pt[3], pt[5] };
+    seg[3].pts = { pt[5], pt[4] };
+    seg[4].pts = { pt[4], pt[2] };
+    seg[5].pts = { pt[2], pt[0] };
+    seg[6].pts = { pt[2], pt[3] };
+    for (int i = 0; i < 7; i++)
+        seg[i].lines = { {0, 1} };
+
+    vector<Mesh> digits = {
+        seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]), // 0
+        seg[1].join(seg[2]), // 1
+        seg[0].join(seg[1]).join(seg[3]).join(seg[4]).join(seg[6]), // 2
+        seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[6]), // 3
+        seg[1].join(seg[2]).join(seg[5]).join(seg[6]), // 4
+        seg[0].join(seg[2]).join(seg[3]).join(seg[5]).join(seg[6]), // 5
+        seg[0].join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]).join(seg[6]), // 6
+        seg[0].join(seg[1]).join(seg[2]), // 7
+        seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]).join(seg[6]), // 8
+        seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[5]).join(seg[6]), // 9
+        seg[6], // -
+    };
+
+    return digits[d];
+}
+
+Mesh drawId(size_t x)
+{
+    vector<int> digits;
+    do
+    {
+        digits.push_back(x % 10);
+        x /= 10;
+    }
+    while (x > 0);
+    float spacing = 0.2f;
+    Mesh m;
+    for (size_t i = 0; i < digits.size(); i++)
+    {
+        Mesh digit = seg7(digits[digits.size() - 1 - i]);
+        Vec6f bb = digit.getBoundingBox();
+        digit = digit.transform(Affine3f().translate(-Vec3f(0, bb[1], 0)));
+        Vec3f tr;
+        if (m.pts.empty())
+            tr = Vec3f();
+        else
+            tr = Vec3f(0, (m.getBoundingBox()[4] + spacing), 0);
+        m = m.join(digit.transform( Affine3f().translate(tr) ));
+    }
+    return m;
+}
+
+
+Mesh drawFromTo(size_t f, size_t t)
+{
+    Mesh m;
+
+    Mesh df = drawId(f);
+    Mesh dp = seg7(10);
+    Mesh dt = drawId(t);
+
+    float spacing = 0.2f;
+    m = m.join(df).join(dp.transform(Affine3f().translate(Vec3f(0, df.getBoundingBox()[4] + spacing, 0))))
+                  .join(dt.transform(Affine3f().translate(Vec3f(0, df.getBoundingBox()[4] + 2*spacing + 1, 0))));
+
+    return m;
+}
+
+Mesh drawPoseGraph(Ptr<detail::PoseGraph> pg)
+{
+    Mesh marker;
+    marker.pts = { {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {1, 1, 0} };
+    marker.lines = { {0, 1}, {0, 2}, {0, 3}, {1, 4} };
+
+    Mesh allMeshes;
+    Affine3f margin = Affine3f().translate(Vec3f(0.1f, 0.1f, 0));
+    std::vector<size_t> ids = pg->getNodesIds();
+    for (const size_t& id : ids)
+    {
+        Affine3f pose = pg->getNodePose(id);
+
+        Mesh m = marker.join(drawId(id).transform(margin, 0.25f)).transform(pose);
+        allMeshes = allMeshes.join(m);
+    }
+
+    // edges
+    margin = Affine3f().translate(Vec3f(0.05f, 0.05f, 0));
+    for (size_t i = 0; i < pg->getNumEdges(); i++)
+    {
+        Affine3f pose = pg->getEdgePose(i);
+        size_t sid = pg->getEdgeStart(i);
+        size_t did = pg->getEdgeEnd(i);
+        Affine3f spose = pg->getNodePose(sid);
+        Affine3f dpose = spose * pose;
+
+        Mesh m = marker.join(drawFromTo(sid, did).transform(margin, 0.125f)).transform(dpose);
+        allMeshes = allMeshes.join(m);
+    }
+
+    return allMeshes;
+}
+
+void writeObj(const std::string& fname, const Mesh& m)
+{
+    // Write edge-only model of how nodes are located in space
+    std::fstream of(fname, std::fstream::out);
+    for (const Point3f& d : m.pts)
+    {
+        of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
+    }
+
+    for (const Vec2i& v : m.lines)
+    {
+        of << "l " << v[0] + 1 << " " << v[1] + 1 << std::endl;
+    }
+
+    of.close();
+}
+
+
+TEST(PoseGraph, simple)
+{
+
+    Ptr<detail::PoseGraph> pg = detail::PoseGraph::create();
+
+    DualQuatf true0(1, 0, 0, 0, 0, 0, 0, 0);
+    DualQuatf true1 = DualQuatf::createFromPitch((float)CV_PI / 3.0f, 10.0f, Vec3f(1, 1.5f, 1.2f), Vec3f());
+
+    DualQuatf pose0 = true0;
+    vector<DualQuatf> noise(7);
+    for (size_t i = 0; i < noise.size(); i++)
+    {
+        float angle = cv::theRNG().uniform(-1.f, 1.f);
+        float shift = cv::theRNG().uniform(-2.f, 2.f);
+        Matx31f axis = Vec3f::randu(0.f, 1.f), moment = Vec3f::randu(0.f, 1.f);
+        noise[i] = DualQuatf::createFromPitch(angle, shift,
+            Vec3f(axis(0), axis(1), axis(2)),
+            Vec3f(moment(0), moment(1), moment(2)));
+    }
+
+    DualQuatf pose1 = noise[0] * true1;
+
+    DualQuatf diff = true1 * true0.inv();
+    vector<DualQuatf> cfrom = { diff, diff * noise[1], noise[2] * diff };
+    DualQuatf diffInv = diff.inv();
+    vector<DualQuatf> cto = { diffInv, diffInv * noise[3], noise[4] * diffInv };
+
+    pg->addNode(123, pose0.toAffine3(), true);
+    pg->addNode(456, pose1.toAffine3(), false);
+
+    Matx66f info = Matx66f::eye();
+    for (int i = 0; i < 3; i++)
+    {
+        pg->addEdge(123, 456, cfrom[i].toAffine3(), info);
+        pg->addEdge(456, 123, cto[i].toAffine3(), info);
+    }
+
+    Mesh allMeshes = drawPoseGraph(pg);
+
+    // Add the "--test_debug" to arguments to see resulting pose graph nodes positions
+    if (cvtest::debugLevel > 0)
+    {
+        writeObj("pg_simple_in.obj", allMeshes);
+    }
+
+    auto r = pg->optimize();
+
+    Mesh after = drawPoseGraph(pg);
+
+    // Add the "--test_debug" to arguments to see resulting pose graph nodes positions
+    if (cvtest::debugLevel > 0)
+    {
+        writeObj("pg_simple_out.obj", after);
+    }
+
+    EXPECT_TRUE(r.found);
+}
+#else
+
+TEST(PoseGraph, sphereG2O)
+{
+    throw SkipTestException("Build with Eigen required for pose graph optimization");
+}
+
+TEST(PoseGraphMST, optimization)
+{
+    throw SkipTestException("Build with Eigen required for pose graph optimization");
+}
+
+TEST(PoseGraph, simple)
+{
+    throw SkipTestException("Build with Eigen required for pose graph optimization");
+}
+#endif
+
+TEST(LevMarq, Rosenbrock)
+{
+    auto f = [](double x, double y) -> double
+    {
+        return (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
+    };
+
+    auto j = [](double x, double y) -> Matx12d
+    {
+        return {/*dx*/ -2.0 + 2.0 * x - 400.0 * x * y + 400.0 * x*x*x,
+                /*dy*/ 200.0 * y - 200.0 * x*x,
+                };
+    };
+
+    LevMarq solver(2, [f, j](InputOutputArray param, OutputArray err, OutputArray jv) -> bool
+    {
+            Vec2d v = param.getMat();
+            double x = v[0], y = v[1];
+            err.create(1, 1, CV_64F);
+            err.getMat().at<double>(0) = f(x, y);
+            if (jv.needed())
+            {
+                jv.create(1, 2, CV_64F);
+                Mat(j(x, y)).copyTo(jv);
+            }
+            return true;
+    },
+    LevMarq::Settings().setGeodesic(true));
+
+    Mat_<double> x (Vec2d(1, 3));
+
+    auto r = solver.run(x);
+
+    EXPECT_TRUE(r.found);
+    EXPECT_LT(r.energy, 0.035);
+    EXPECT_LE(r.iters, 17);
+}
+
+
+}} // namespace

modules/ptcloud/test/test_precomp.hpp

@@ -0,0 +1,21 @@
+// 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_TEST_PRECOMP_HPP__
+#define __OPENCV_TEST_PRECOMP_HPP__
+
+#include <functional>
+#include <numeric>
+
+#include "opencv2/ts.hpp"
+#include "opencv2/geometry.hpp"
+#include "opencv2/ptcloud.hpp"
+#include <opencv2/core/utils/logger.hpp>
+#include "opencv2/ptcloud/depth.hpp"
+#include "opencv2/ptcloud/odometry.hpp"
+
+#ifdef HAVE_OPENCL
+#include <opencv2/core/ocl.hpp>
+#endif
+
+#endif

modules/ptcloud/test/test_registration.cpp

@@ -0,0 +1,107 @@
+// 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_WillowGarage.md file found in this module's directory
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+class RgbdDepthRegistrationTest
+{
+public:
+    RgbdDepthRegistrationTest() { }
+    ~RgbdDepthRegistrationTest() { }
+
+    void run()
+    {
+        // Test all three input types for no-op registrations (where a depth image is registered to itself)
+        noOpRandomRegistrationTest<unsigned short>(100, 2500);
+        noOpRandomRegistrationTest<float>(0.1f, 2.5f);
+        noOpRandomRegistrationTest<double>(0.1, 2.5);
+
+        // Test sentinel value handling, occlusion, and dilation
+        {
+            // K from a VGA Kinect
+            Mat K = (Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.);
+
+            int width = 640, height = 480;
+
+            // All elements are zero except for first two along the diagonal
+            Mat_<unsigned short> vgaDepth(height, width, (unsigned short)0);
+            vgaDepth(0, 0) = 1001;
+            vgaDepth(1, 1) = 1000;
+
+            Mat_<unsigned short> registeredDepth;
+            registerDepth(K, K, Mat(), Matx44f::eye(), vgaDepth, Size(width, height), registeredDepth, true);
+
+            // We expect the closer depth of 1000 to occlude the more distant depth and occupy the
+            // upper four left pixels in the depth image because of dilation
+            Mat_<unsigned short> expectedResult(height, width, (unsigned short)0);
+            expectedResult(0, 0) = 1000;
+            expectedResult(0, 1) = 1000;
+            expectedResult(1, 0) = 1000;
+            expectedResult(1, 1) = 1000;
+
+            Mat ad;
+            absdiff(registeredDepth, expectedResult, ad);
+            ASSERT_GT(std::numeric_limits<double>::min(), abs(sum(ad)[0])) << "Dilation and occlusion";
+        }
+    }
+
+    template <class DepthDepth>
+    void noOpRandomRegistrationTest(DepthDepth minDepth, DepthDepth maxDepth)
+    {
+        // K from a VGA Kinect
+        Mat K = (Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.);
+
+        // Create a random depth image
+        RNG rng;
+        Mat_<DepthDepth> randomVGADepth(480, 640);
+        rng.fill(randomVGADepth, RNG::UNIFORM, minDepth, maxDepth);
+
+        Mat registeredDepth;
+        registerDepth(K, K, Mat(), Matx44f::eye(), randomVGADepth, Size(640, 480), registeredDepth);
+
+        // Check per-pixel relative difference
+
+        Mat ad;
+        absdiff(registeredDepth, randomVGADepth, ad);
+        Mat mmin;
+        cv::min(registeredDepth, randomVGADepth, mmin);
+        Mat rel = ad / mmin;
+        double maxDiff = cv::norm(rel, NORM_INF);
+        ASSERT_GT(1e-07, maxDiff) << "No-op registration";
+    }
+};
+
+TEST(RGBD_DepthRegistration, compute)
+{
+    RgbdDepthRegistrationTest test;
+    test.run();
+}
+
+TEST(RGBD_DepthRegistration, issue_2234)
+{
+    Matx33f intrinsicsDepth(100, 0,  50, 0, 100, 50, 0, 0, 1);
+    Matx33f intrinsicsColor(100, 0, 200, 0, 100, 50, 0, 0, 1);
+
+    Mat_<float> depthMat(100, 100, (float)0.);
+    for(int i = 1; i <= 100; i++)
+    {
+        for(int j = 1; j <= 100; j++)
+            depthMat(i-1,j-1) = (float)j;
+    }
+
+    Mat registeredDepth;
+    registerDepth(intrinsicsDepth, intrinsicsColor, Mat(), Matx44f::eye(), depthMat, Size(400, 100), registeredDepth);
+
+    Rect roi( 150, 0, 100, 100 );
+    Mat subM(registeredDepth,roi);
+
+    EXPECT_EQ(0, cvtest::norm(subM, depthMat, NORM_INF));
+}
+
+
+}} // namespace

modules/ptcloud/test/test_rendering.cpp

@@ -0,0 +1,946 @@
+// 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
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+using namespace cv;
+
+// that was easier than using CV_ENUM() macro
+namespace
+{
+using namespace cv;
+struct CullingModeEnum
+{
+    static const std::array<TriangleCullingMode, 3> vals;
+    static const std::array<std::string, 3> svals;
+
+    CullingModeEnum(TriangleCullingMode v = RASTERIZE_CULLING_NONE) : val(v) {}
+    operator TriangleCullingMode() const { return val; }
+    void PrintTo(std::ostream *os) const
+    {
+        int v = int(val);
+        if (v >= 0 && v < (int)vals.size())
+        {
+            *os << svals[v];
+        }
+        else
+        {
+            *os << "UNKNOWN";
+        }
+    }
+    static ::testing::internal::ParamGenerator<CullingModeEnum> all()
+    {
+        return ::testing::Values(CullingModeEnum(vals[0]),
+                                 CullingModeEnum(vals[1]),
+                                 CullingModeEnum(vals[2]));
+    }
+
+private:
+    TriangleCullingMode val;
+};
+
+const std::array<TriangleCullingMode, 3> CullingModeEnum::vals
+{
+    RASTERIZE_CULLING_NONE,
+    RASTERIZE_CULLING_CW,
+    RASTERIZE_CULLING_CCW
+};
+const std::array<std::string, 3> CullingModeEnum::svals
+{
+    std::string("None"),
+    std::string("CW"),
+    std::string("CCW")
+};
+
+static inline void PrintTo(const CullingModeEnum &t, std::ostream *os) { t.PrintTo(os); }
+}
+
+// that was easier than using CV_ENUM() macro
+namespace
+{
+using namespace cv;
+struct ShadingTypeEnum
+{
+    static const std::array<TriangleShadingType, 3> vals;
+    static const std::array<std::string, 3> svals;
+
+    ShadingTypeEnum(TriangleShadingType v = RASTERIZE_SHADING_WHITE) : val(v) {}
+    operator TriangleShadingType() const { return val; }
+    void PrintTo(std::ostream *os) const
+    {
+        int v = int(val);
+        if (v >= 0 && v < (int)vals.size())
+        {
+            *os << svals[v];
+        }
+        else
+        {
+            *os << "UNKNOWN";
+        }
+    }
+    static ::testing::internal::ParamGenerator<ShadingTypeEnum> all()
+    {
+        return ::testing::Values(ShadingTypeEnum(vals[0]),
+                                 ShadingTypeEnum(vals[1]),
+                                 ShadingTypeEnum(vals[2]));
+    }
+
+private:
+    TriangleShadingType val;
+};
+
+const std::array<TriangleShadingType, 3> ShadingTypeEnum::vals
+{
+    RASTERIZE_SHADING_WHITE,
+    RASTERIZE_SHADING_FLAT,
+    RASTERIZE_SHADING_SHADED
+};
+const std::array<std::string, 3> ShadingTypeEnum::svals
+{
+    std::string("White"),
+    std::string("Flat"),
+    std::string("Shaded")
+};
+
+static inline void PrintTo(const ShadingTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
+}
+
+
+enum class ModelType
+{
+    Empty = 0,
+    File = 1,
+    Clipping = 2,
+    Color = 3,
+    Centered = 4
+};
+
+// that was easier than using CV_ENUM() macro
+namespace
+{
+using namespace cv;
+struct ModelTypeEnum
+{
+    static const std::array<ModelType, 5> vals;
+    static const std::array<std::string, 5> svals;
+
+    ModelTypeEnum(ModelType v = ModelType::Empty) : val(v) {}
+    operator ModelType() const { return val; }
+    void PrintTo(std::ostream *os) const
+    {
+        int v = int(val);
+        if (v >= 0 && v < (int)vals.size())
+        {
+            *os << svals[v];
+        }
+        else
+        {
+            *os << "UNKNOWN";
+        }
+    }
+    static ::testing::internal::ParamGenerator<ModelTypeEnum> all()
+    {
+        return ::testing::Values(ModelTypeEnum(vals[0]),
+                                 ModelTypeEnum(vals[1]),
+                                 ModelTypeEnum(vals[2]),
+                                 ModelTypeEnum(vals[3]),
+                                 ModelTypeEnum(vals[4]));
+    }
+
+private:
+    ModelType val;
+};
+
+const std::array<ModelType, 5> ModelTypeEnum::vals
+{
+    ModelType::Empty,
+    ModelType::File,
+    ModelType::Clipping,
+    ModelType::Color,
+    ModelType::Centered
+};
+const std::array<std::string, 5> ModelTypeEnum::svals
+{
+    std::string("Empty"),
+    std::string("File"),
+    std::string("Clipping"),
+    std::string("Color"),
+    std::string("Centered")
+};
+
+static inline void PrintTo(const ModelTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
+}
+
+template<typename T>
+std::string printEnum(T v)
+{
+    std::ostringstream ss;
+    v.PrintTo(&ss);
+    return ss.str();
+}
+
+
+static Matx44d lookAtMatrixCal(const Vec3d& position, const Vec3d& lookat, const Vec3d& upVector)
+{
+    Vec3d w = cv::normalize(position - lookat);
+    Vec3d u = cv::normalize(upVector.cross(w));
+
+    Vec3d v = w.cross(u);
+
+    Matx44d res(u[0], u[1], u[2],   0,
+                v[0], v[1], v[2],   0,
+                w[0], w[1], w[2],   0,
+                   0,    0,    0,   1.0);
+
+    Matx44d translate(1.0,   0,   0, -position[0],
+                        0, 1.0,   0, -position[1],
+                        0,   0, 1.0, -position[2],
+                        0,   0,   0,          1.0);
+    res = res * translate;
+
+    return res;
+}
+
+
+static void generateNormals(const std::vector<Vec3f>& points, const std::vector<std::vector<int>>& indices,
+                            std::vector<Vec3f>& normals)
+{
+    std::vector<std::vector<Vec3f>> preNormals(points.size(), std::vector<Vec3f>());
+
+    for (const auto& tri : indices)
+    {
+        Vec3f p0 = points[tri[0]];
+        Vec3f p1 = points[tri[1]];
+        Vec3f p2 = points[tri[2]];
+
+        Vec3f cross = cv::normalize((p1 - p0).cross(p2 - p0));
+        for (int i = 0; i < 3; i++)
+        {
+            preNormals[tri[i]].push_back(cross);
+        }
+    }
+
+    normals.reserve(points.size());
+    for (const auto& pn : preNormals)
+    {
+        Vec3f sum { };
+        for (const auto& n : pn)
+        {
+            sum += n;
+        }
+        normals.push_back(cv::normalize(sum));
+    }
+}
+
+// load model once and keep it in static memory
+static void getModelOnce(const std::string& objectPath, std::vector<Vec3f>& vertices,
+                         std::vector<Vec3i>& indices, std::vector<Vec3f>& colors)
+{
+    static bool load = false;
+    static std::vector<Vec3f> vert, col;
+    static std::vector<Vec3i> ind;
+
+    if (!load)
+    {
+        std::vector<vector<int>> indvec;
+        // using per-vertex normals as colors
+        loadMesh(objectPath, vert, indvec);
+        generateNormals(vert, indvec, col);
+
+        for (const auto &vec : indvec)
+        {
+            ind.push_back({vec[0], vec[1], vec[2]});
+        }
+
+        for (auto &color : col)
+        {
+            color = Vec3f(abs(color[0]), abs(color[1]), abs(color[2]));
+        }
+
+        load = true;
+    }
+
+    vertices = vert;
+    colors = col;
+    indices = ind;
+}
+
+class ModelData
+{
+public:
+    ModelData(ModelType type = ModelType::Empty)
+    {
+        switch (type)
+        {
+        case ModelType::Empty:
+        {
+            position = Vec3d(0.0, 0.0, 0.0);
+            lookat   = Vec3d(0.0, 0.0, 0.0);
+            upVector = Vec3d(0.0, 1.0, 0.0);
+
+            fovy = 45.0;
+
+            vertices = std::vector<Vec3f>(4, {2.0f, 0, -2.0f});
+            colors   = std::vector<Vec3f>(4, {0, 0, 1.0f});
+            indices = { };
+        }
+        break;
+        case ModelType::File:
+        {
+            string objectPath = findDataFile("viz/dragon.ply");
+
+            position = Vec3d( 1.9, 0.4, 1.3);
+            lookat   = Vec3d( 0.0, 0.0, 0.0);
+            upVector = Vec3d( 0.0, 1.0, 0.0);
+
+            fovy = 45.0;
+
+            getModelOnce(objectPath, vertices, indices, colors);
+        }
+        break;
+        case ModelType::Clipping:
+        {
+            position = Vec3d(0.0, 0.0, 5.0);
+            lookat   = Vec3d(0.0, 0.0, 0.0);
+            upVector = Vec3d(0.0, 1.0, 0.0);
+
+            fovy = 45.0;
+
+            vertices =
+            {
+                { 2.0,  0.0, -2.0}, { 0.0, -6.0, -2.0}, {-2.0,  0.0, -2.0},
+                { 3.5, -1.0, -5.0}, { 2.5, -2.5, -5.0}, {-1.0,  1.0, -5.0},
+                {-6.5, -1.0, -3.0}, {-2.5, -2.0, -3.0}, { 1.0,  1.0, -5.0},
+            };
+
+            indices = { {0, 1, 2}, {3, 4, 5}, {6, 7, 8} };
+
+            Vec3f col1(217.0, 238.0, 185.0);
+            Vec3f col2(185.0, 217.0, 238.0);
+            Vec3f col3(150.0,  10.0, 238.0);
+
+            col1 *= (1.f / 255.f);
+            col2 *= (1.f / 255.f);
+            col3 *= (1.f / 255.f);
+
+            colors =
+            {
+                col1, col2, col3,
+                col2, col3, col1,
+                col3, col1, col2,
+            };
+        }
+        break;
+        case ModelType::Centered:
+        {
+            position = Vec3d(0.0, 0.0, 5.0);
+            lookat   = Vec3d(0.0, 0.0, 0.0);
+            upVector = Vec3d(0.0, 1.0, 0.0);
+
+            fovy = 45.0;
+
+            vertices =
+            {
+                { 2.0,  0.0, -2.0}, { 0.0, -2.0, -2.0}, {-2.0,  0.0, -2.0},
+                { 3.5, -1.0, -5.0}, { 2.5, -1.5, -5.0}, {-1.0,  0.5, -5.0},
+            };
+
+            indices = { {0, 1, 2}, {3, 4, 5} };
+
+            Vec3f col1(217.0, 238.0, 185.0);
+            Vec3f col2(185.0, 217.0, 238.0);
+
+            col1 *= (1.f / 255.f);
+            col2 *= (1.f / 255.f);
+
+            colors =
+            {
+                col1, col2, col1,
+                col2, col1, col2,
+            };
+        }
+        break;
+        case ModelType::Color:
+        {
+            position = Vec3d(0.0, 0.0, 5.0);
+            lookat   = Vec3d(0.0, 0.0, 0.0);
+            upVector = Vec3d(0.0, 1.0, 0.0);
+
+            fovy = 60.0;
+
+            vertices =
+            {
+                { 2.0,  0.0, -2.0},
+                { 0.0,  2.0, -3.0},
+                {-2.0,  0.0, -2.0},
+                { 0.0, -2.0,  1.0},
+            };
+
+            indices = { {0, 1, 2}, {0, 2, 3} };
+
+            colors =
+            {
+                { 0.0f, 0.0f, 1.0f},
+                { 0.0f, 1.0f, 0.0f},
+                { 1.0f, 0.0f, 0.0f},
+                { 0.0f, 1.0f, 0.0f},
+            };
+        }
+        break;
+
+        default:
+            CV_Error(Error::StsBadArg, "Unknown model type");
+            break;
+        }
+    }
+
+    Vec3d position;
+    Vec3d lookat;
+    Vec3d upVector;
+
+    double fovy;
+
+    std::vector<Vec3f> vertices;
+    std::vector<Vec3i> indices;
+    std::vector<Vec3f> colors;
+};
+
+
+void compareDepth(const cv::Mat& gt, const cv::Mat& mat, cv::Mat& diff, double zFar, double scale,
+                  double maskThreshold, double normInfThreshold, double normL2Threshold)
+{
+    ASSERT_EQ(CV_16UC1, gt.type());
+    ASSERT_EQ(CV_16UC1, mat.type());
+    ASSERT_EQ(gt.size(), mat.size());
+
+    Mat gtMask  = gt  < zFar*scale;
+    Mat matMask = mat < zFar*scale;
+
+    Mat diffMask = gtMask != matMask;
+    int nzDepthDiff = cv::countNonZero(diffMask);
+    EXPECT_LE(nzDepthDiff, maskThreshold);
+
+    Mat jointMask = gtMask & matMask;
+    int nzJointMask = cv::countNonZero(jointMask);
+    double normInfDepth = cv::norm(gt, mat, cv::NORM_INF, jointMask);
+    EXPECT_LE(normInfDepth, normInfThreshold);
+    double normL2Depth = nzJointMask ? (cv::norm(gt, mat, cv::NORM_L2, jointMask) / nzJointMask) : 0;
+    EXPECT_LE(normL2Depth, normL2Threshold);
+
+    // add --test_debug to output differences
+    if (debugLevel > 0)
+    {
+        std::cout << "nzDepthDiff: "  << nzDepthDiff  << " vs " << maskThreshold << std::endl;
+        std::cout << "normInfDepth: " << normInfDepth << " vs " << normInfThreshold << std::endl;
+        std::cout << "normL2Depth: "  << normL2Depth  << " vs " << normL2Threshold << std::endl;
+    }
+
+    diff = (gt - mat) + (1 << 15);
+}
+
+
+void compareRGB(const cv::Mat& gt, const cv::Mat& mat, cv::Mat& diff, double normInfThreshold, double normL2Threshold)
+{
+    ASSERT_EQ(CV_32FC3, gt.type());
+    ASSERT_EQ(CV_32FC3, mat.type());
+    ASSERT_EQ(gt.size(), mat.size());
+
+    double normInfRgb = cv::norm(gt, mat, cv::NORM_INF);
+    EXPECT_LE(normInfRgb, normInfThreshold);
+    double normL2Rgb = cv::norm(gt, mat, cv::NORM_L2) / gt.total();
+    EXPECT_LE(normL2Rgb, normL2Threshold);
+    // add --test_debug to output differences
+    if (debugLevel > 0)
+    {
+        std::cout << "normInfRgb: " << normInfRgb << " vs " << normInfThreshold << std::endl;
+        std::cout << "normL2Rgb: " << normL2Rgb << " vs " << normL2Threshold << std::endl;
+    }
+    diff = (gt - mat) * 0.5 + 0.5;
+}
+
+
+struct RenderTestThresholds
+{
+    RenderTestThresholds(
+        double _rgbInfThreshold,
+        double _rgbL2Threshold,
+        double _depthMaskThreshold,
+        double _depthInfThreshold,
+        double _depthL2Threshold) :
+        rgbInfThreshold(_rgbInfThreshold),
+        rgbL2Threshold(_rgbL2Threshold),
+        depthMaskThreshold(_depthMaskThreshold),
+        depthInfThreshold(_depthInfThreshold),
+        depthL2Threshold(_depthL2Threshold)
+    { }
+
+    double rgbInfThreshold;
+    double rgbL2Threshold;
+    double depthMaskThreshold;
+    double depthInfThreshold;
+    double depthL2Threshold;
+};
+
+// resolution, shading type, culling mode, model type, float type, index type
+typedef std::tuple<std::tuple<int, int>, ShadingTypeEnum, CullingModeEnum, ModelTypeEnum, MatDepth, MatDepth> RenderTestParamType;
+
+class RenderingTest : public ::testing::TestWithParam<RenderTestParamType>
+{
+protected:
+    void SetUp() override
+    {
+        params = GetParam();
+        auto wh = std::get<0>(params);
+        width = std::get<0>(wh);
+        height = std::get<1>(wh);
+        shadingType = std::get<1>(params);
+        cullingMode = std::get<2>(params);
+        modelType = std::get<3>(params);
+        modelData = ModelData(modelType);
+        ftype = std::get<4>(params);
+        itype = std::get<5>(params);
+
+        zNear = 0.1, zFar = 50.0;
+        depthScale = 1000.0;
+
+        depth_buf = Mat(height, width, ftype, zFar);
+        color_buf = Mat(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
+
+        cameraPose = lookAtMatrixCal(modelData.position, modelData.lookat, modelData.upVector);
+        fovYradians = modelData.fovy * (CV_PI / 180.0);
+
+        verts = Mat(modelData.vertices);
+        verts.convertTo(verts, ftype);
+
+        if (shadingType != RASTERIZE_SHADING_WHITE)
+        {
+            // let vertices be in BGR format to avoid later color conversions
+            // mixChannels() does not support in-place operation
+            colors = Mat(modelData.colors);
+            colors.convertTo(colors, ftype);
+            cv::mixChannels(colors.clone(), colors, {0, 2, 1, 1, 2, 0});
+        }
+
+        indices = Mat(modelData.indices);
+        if (itype != CV_32S)
+        {
+            indices.convertTo(indices, itype);
+        }
+
+        settings = TriangleRasterizeSettings().setCullingMode(cullingMode).setShadingType(shadingType);
+
+        triangleRasterize(verts, indices, colors, color_buf, depth_buf,
+                          cameraPose, fovYradians, zNear, zFar, settings);
+    }
+
+public:
+    RenderTestParamType params;
+    int width, height;
+    double zNear, zFar, depthScale;
+
+    Mat depth_buf, color_buf;
+
+    Mat verts, colors, indices;
+    Matx44d cameraPose;
+    double fovYradians;
+    TriangleRasterizeSettings settings;
+
+    ModelData modelData;
+    ModelTypeEnum modelType;
+    ShadingTypeEnum shadingType;
+    CullingModeEnum cullingMode;
+    int ftype, itype;
+};
+
+
+// depth-only or RGB-only rendering should produce the same result as usual rendering
+TEST_P(RenderingTest, noArrays)
+{
+    Mat depthOnly(height, width, ftype, zFar);
+    Mat colorOnly(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
+
+    triangleRasterizeDepth(verts, indices, depthOnly, cameraPose, fovYradians, zNear, zFar, settings);
+    triangleRasterizeColor(verts, indices, colors, colorOnly, cameraPose, fovYradians, zNear, zFar, settings);
+
+    Mat rgbDiff, depthDiff;
+    compareRGB(color_buf, colorOnly, rgbDiff, 0, 0);
+    depth_buf.convertTo(depth_buf, CV_16U, depthScale);
+    depthOnly.convertTo(depthOnly, CV_16U, depthScale);
+    compareDepth(depth_buf, depthOnly, depthDiff, zFar, depthScale, 0, 0, 0);
+
+    // add --test_debug to output resulting images
+    if (debugLevel > 0)
+    {
+        std::string modelName   = printEnum(modelType);
+        std::string shadingName = printEnum(shadingType);
+        std::string cullingName = printEnum(cullingMode);
+        std::string suffix = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingName.c_str());
+
+        std::string outColorPath = "noarray_color_image_" + suffix + "_" + shadingName + ".png";
+        std::string outDepthPath = "noarray_depth_image_" + suffix + "_" + shadingName + ".png";
+
+        imwrite(outColorPath, color_buf * 255.f);
+        imwrite(outDepthPath, depth_buf);
+        imwrite("diff_" + outColorPath, rgbDiff * 255.f);
+        imwrite("diff_" + outDepthPath, depthDiff);
+    }
+}
+
+
+// passing the same parameters in float should give the same result
+TEST_P(RenderingTest, floatParams)
+{
+    Mat depth_buf2(height, width, ftype, zFar);
+    Mat color_buf2(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
+
+    // cameraPose can also be float, checking it
+    triangleRasterize(verts, indices, colors, color_buf2, depth_buf2,
+                      Matx44f(cameraPose), (float)fovYradians, (float)zNear, (float)zFar, settings);
+
+    RenderTestThresholds thr(0, 0, 0, 0, 0);
+    switch (modelType)
+    {
+        case ModelType::Empty: break;
+        case ModelType::Color: break;
+        case ModelType::Clipping:
+            if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CW)
+            {
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.00127;
+            }
+            else if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_NONE)
+            {
+                thr.rgbInfThreshold = 3e-7;
+                thr.rgbL2Threshold = 1.86e-10;
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.000406;
+            }
+            else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
+            {
+                thr.rgbInfThreshold = 2.39e-07;
+                thr.rgbL2Threshold = 1.86e-10;
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.0016;
+            }
+            else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
+            {
+                thr.rgbInfThreshold = 0.934;
+                thr.rgbL2Threshold = 0.000102;
+                thr.depthMaskThreshold = 21;
+            }
+            else if (width == 640 && height == 480 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
+            {
+                thr.rgbL2Threshold = 1;
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.000248;
+            }
+            else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
+            {
+                thr.rgbInfThreshold = 0.934;
+                thr.rgbL2Threshold = 3.18e-5;
+                thr.depthMaskThreshold = 114;
+            }
+            break;
+        case ModelType::File:
+            thr.depthInfThreshold = 1;
+            if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CCW)
+            {
+                thr.rgbInfThreshold = 0.000229;
+                thr.rgbL2Threshold = 6.37e-09;
+                thr.depthL2Threshold = 0.00043;
+            }
+            else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
+            {
+                thr.rgbInfThreshold = 0.000277;
+                thr.rgbL2Threshold = 1.8e-09;
+                thr.depthL2Threshold = 0.000124;
+            }
+            else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
+            {
+                thr.depthL2Threshold = 0.000124;
+            }
+            break;
+        case ModelType::Centered:
+            if (shadingType == RASTERIZE_SHADING_SHADED && cullingMode != RASTERIZE_CULLING_CW)
+            {
+                thr.rgbInfThreshold = 3.58e-07;
+                thr.rgbL2Threshold  = 1.51e-10;
+            }
+            break;
+    }
+
+    Mat rgbDiff, depthDiff;
+    compareRGB(color_buf, color_buf2, rgbDiff, thr.rgbInfThreshold, thr.rgbL2Threshold);
+    depth_buf.convertTo(depth_buf, CV_16U, depthScale);
+    depth_buf2.convertTo(depth_buf2, CV_16U, depthScale);
+    compareDepth(depth_buf, depth_buf2, depthDiff, zFar, depthScale, thr.depthMaskThreshold, thr.depthInfThreshold, thr.depthL2Threshold);
+
+    // add --test_debug to output resulting images
+    if (debugLevel > 0)
+    {
+        std::string modelName   = printEnum(modelType);
+        std::string shadingName = printEnum(shadingType);
+        std::string cullingName = printEnum(cullingMode);
+        std::string suffix = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingName.c_str());
+
+        std::string outColorPath = "float_color_image_" + suffix + "_" + shadingName + ".png";
+        std::string outDepthPath = "float_depth_image_" + suffix + "_" + shadingName + ".png";
+
+        imwrite(outColorPath, color_buf * 255.f);
+        imwrite(outDepthPath, depth_buf);
+        imwrite("diff_" + outColorPath, rgbDiff * 255.f);
+        imwrite("diff_" + outDepthPath, depthDiff);
+    }
+}
+
+
+// some culling options produce the same pictures, let's join them
+TriangleCullingMode findSameCulling(ModelType modelType, TriangleShadingType shadingType, TriangleCullingMode cullingMode, bool forRgb)
+{
+    TriangleCullingMode sameCullingMode = cullingMode;
+
+    if ((modelType == ModelType::Centered && cullingMode == RASTERIZE_CULLING_CCW) ||
+        (modelType == ModelType::Color    && cullingMode == RASTERIZE_CULLING_CW)  ||
+        (modelType == ModelType::File     && shadingType == RASTERIZE_SHADING_WHITE && forRgb) ||
+        (modelType == ModelType::File     && cullingMode == RASTERIZE_CULLING_CW))
+    {
+        sameCullingMode = RASTERIZE_CULLING_NONE;
+    }
+
+    return sameCullingMode;
+}
+
+// compare rendering results to the ones produced by samples/opengl/opengl_testdata_generator app
+TEST_P(RenderingTest, accuracy)
+{
+    depth_buf.convertTo(depth_buf, CV_16U, depthScale);
+
+    if (modelType == ModelType::Empty ||
+       (modelType == ModelType::Centered && cullingMode == RASTERIZE_CULLING_CW) ||
+       (modelType == ModelType::Color    && cullingMode == RASTERIZE_CULLING_CCW))
+    {
+        // empty image case
+        EXPECT_EQ(0, cv::norm(color_buf, NORM_INF));
+
+        Mat depthDiff;
+        absdiff(depth_buf, Scalar(zFar * depthScale), depthDiff);
+        EXPECT_EQ(0, cv::norm(depthDiff, cv::NORM_INF));
+    }
+    else
+    {
+        RenderTestThresholds thr(0, 0, 0, 0, 0);
+        switch (modelType)
+        {
+        case ModelType::Centered:
+            if (shadingType == RASTERIZE_SHADING_SHADED)
+            {
+                thr.rgbInfThreshold = 0.00218;
+                thr.rgbL2Threshold = 2.85e-06;
+            }
+            break;
+        case ModelType::Clipping:
+            if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CW)
+            {
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.00163;
+            }
+            else if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_NONE)
+            {
+                thr.rgbInfThreshold = 0.934;
+                thr.rgbL2Threshold = 8.03E-05;
+                thr.depthMaskThreshold = 23;
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.000555;
+            }
+            else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
+            {
+                thr.rgbInfThreshold = 0.0022;
+                thr.rgbL2Threshold = 2.54E-06;
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.00175;
+            }
+            else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
+            {
+                thr.rgbInfThreshold = 0.934;
+                thr.rgbL2Threshold = 0.000102;
+                thr.depthMaskThreshold = 21;
+            }
+            else if (width == 640 && height == 480 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
+            {
+                thr.rgbInfThreshold = 1;
+                thr.rgbL2Threshold = 3.95E-05;
+                thr.depthMaskThreshold = 49;
+                thr.depthInfThreshold = 1;
+                thr.depthL2Threshold = 0.000269;
+            }
+            else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
+            {
+                thr.rgbInfThreshold = 0.934;
+                thr.rgbL2Threshold = 3.27e-5;
+                thr.depthMaskThreshold = 121;
+            }
+            break;
+        case ModelType::Color:
+            thr.depthInfThreshold = 1;
+            if (width == 320 && height == 240)
+            {
+                thr.depthL2Threshold = 0.00103;
+            }
+            else if (width == 256 && height == 256)
+            {
+                thr.depthL2Threshold = 0.000785;
+            }
+            if (shadingType == RASTERIZE_SHADING_SHADED)
+            {
+                thr.rgbInfThreshold = 0.0022;
+                thr.rgbL2Threshold = 3.13e-06;
+            }
+            break;
+        case ModelType::File:
+            if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CCW)
+            {
+                thr.rgbInfThreshold = 0.836;
+                thr.rgbL2Threshold = 2.08e-05;
+                thr.depthMaskThreshold = 1;
+                thr.depthInfThreshold = 99;
+                thr.depthL2Threshold = 0.00544;
+            }
+            else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
+            {
+                thr.rgbInfThreshold = 0.973;
+                thr.rgbL2Threshold = 5.2e-06;
+                thr.depthMaskThreshold = 4;
+                thr.depthInfThreshold = 258;
+                thr.depthL2Threshold = 0.00228;
+            }
+            else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
+            {
+                thr.rgbInfThreshold = 1;
+                thr.rgbL2Threshold = 7.07e-06;
+                thr.depthMaskThreshold = 4;
+                thr.depthInfThreshold = 258;
+                thr.depthL2Threshold = 0.00228;
+            }
+            break;
+        default:
+            break;
+        }
+
+        CullingModeEnum cullingModeRgb   = findSameCulling(modelType, shadingType, cullingMode, true);
+        CullingModeEnum cullingModeDepth = findSameCulling(modelType, shadingType, cullingMode, false);
+
+        std::string modelName        = printEnum(modelType);
+        std::string shadingName      = printEnum(shadingType);
+        std::string cullingName      = printEnum(cullingMode);
+        std::string cullingRgbName   = printEnum(cullingModeRgb);
+        std::string cullingDepthName = printEnum(cullingModeDepth);
+
+        std::string path = findDataDirectory("rendering");
+        std::string suffix      = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingName.c_str());
+        std::string suffixRgb   = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingRgbName.c_str());
+        std::string suffixDepth = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingDepthName.c_str());
+        std::string gtPathColor = path + "/example_image_" + suffixRgb + "_" + shadingName + ".png";
+        std::string gtPathDepth = path + "/depth_image_"   + suffixDepth + ".png";
+
+        Mat rgbDiff, depthDiff;
+        Mat groundTruthColor = imread(gtPathColor);
+        groundTruthColor.convertTo(groundTruthColor, CV_32F, (1.f / 255.f));
+        compareRGB(groundTruthColor, color_buf, rgbDiff, thr.rgbInfThreshold, thr.rgbL2Threshold);
+
+        Mat groundTruthDepth = imread(gtPathDepth, cv::IMREAD_GRAYSCALE | cv::IMREAD_ANYDEPTH);
+        compareDepth(groundTruthDepth, depth_buf, depthDiff, zFar, depthScale, thr.depthMaskThreshold, thr.depthInfThreshold, thr.depthL2Threshold);
+
+        // add --test_debug to output resulting images
+        if (debugLevel > 0)
+        {
+            std::string outColorPath = "color_image_" + suffix + "_" + shadingName + ".png";
+            std::string outDepthPath = "depth_image_" + suffix + "_" + shadingName + ".png";
+            imwrite(outColorPath, color_buf * 255.f);
+            imwrite(outDepthPath, depth_buf);
+            imwrite("diff_" + outColorPath, rgbDiff * 255.f);
+            imwrite("diff_" + outDepthPath, depthDiff);
+        }
+    }
+}
+
+
+// drawing model as a whole or as two halves should give the same result
+TEST_P(RenderingTest, keepDrawnData)
+{
+    if (modelType != ModelType::Empty)
+    {
+        Mat depth_buf2(height, width, ftype, zFar);
+        Mat color_buf2(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
+
+        Mat idx1, idx2;
+        int nTriangles = (int)indices.total();
+        idx1 = indices.reshape(3, 1)(Range::all(), Range(0, nTriangles / 2));
+        idx2 = indices.reshape(3, 1)(Range::all(), Range(nTriangles / 2, nTriangles));
+
+        triangleRasterize(verts, idx1, colors, color_buf2, depth_buf2, cameraPose, fovYradians, zNear, zFar, settings);
+        triangleRasterize(verts, idx2, colors, color_buf2, depth_buf2, cameraPose, fovYradians, zNear, zFar, settings);
+
+        Mat rgbDiff, depthDiff;
+        compareRGB(color_buf, color_buf2, rgbDiff, 0, 0);
+        depth_buf.convertTo(depth_buf, CV_16U, depthScale);
+        depth_buf2.convertTo(depth_buf2, CV_16U, depthScale);
+        compareDepth(depth_buf, depth_buf2, depthDiff, zFar, depthScale, 0, 0, 0);
+    }
+}
+
+
+TEST_P(RenderingTest, glCompatibleDepth)
+{
+    Mat depth_buf2(height, width, ftype, 1.0);
+
+    triangleRasterizeDepth(verts, indices, depth_buf2, cameraPose, fovYradians, zNear, zFar,
+                           settings.setGlCompatibleMode(RASTERIZE_COMPAT_INVDEPTH));
+
+    Mat convertedDepth(height, width, ftype);
+    // map from [0, 1] to [zNear, zFar]
+    double scaleNear = (1.0 / zNear);
+    double scaleFar  = (1.0 / zFar);
+    for (int y = 0; y < height; y++)
+    {
+        for (int x = 0; x < width; x++)
+        {
+            double z = (double)depth_buf2.at<float>(y, x);
+            convertedDepth.at<float>(y, x) = (float)(1.0 / ((1.0 - z) * scaleNear + z * scaleFar ));
+        }
+    }
+
+    double normL2Diff = cv::norm(depth_buf, convertedDepth, cv::NORM_L2) / (height * width);
+    const double normL2Threshold = 5.53e-10;
+    EXPECT_LE(normL2Diff, normL2Threshold);
+    // add --test_debug to output differences
+    if (debugLevel > 0)
+    {
+        std::cout << "normL2Diff: "  << normL2Diff  << " vs " << normL2Threshold << std::endl;
+    }
+}
+
+
+INSTANTIATE_TEST_CASE_P(Rendering, RenderingTest, ::testing::Values(
+    RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Centered,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Centered,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_WHITE,  RASTERIZE_CULLING_NONE, ModelType::Centered,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(640, 480), RASTERIZE_SHADING_FLAT,   RASTERIZE_CULLING_NONE, ModelType::Centered,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_FLAT,   RASTERIZE_CULLING_CW,   ModelType::Color,     CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Color,     CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Color,     CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_WHITE,  RASTERIZE_CULLING_NONE, ModelType::Color,     CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_FLAT,   RASTERIZE_CULLING_CW,   ModelType::Clipping,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Clipping,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_FLAT,   RASTERIZE_CULLING_CCW,  ModelType::Clipping,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_CW,   ModelType::Clipping,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(640, 480), RASTERIZE_SHADING_WHITE,  RASTERIZE_CULLING_NONE, ModelType::Clipping,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(700, 700), RASTERIZE_SHADING_FLAT,   RASTERIZE_CULLING_CCW,  ModelType::Clipping,  CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_CCW,  ModelType::File,      CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(700, 700), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_CW,   ModelType::File,      CV_32F, CV_32S },
+    RenderTestParamType { std::make_tuple(700, 700), RASTERIZE_SHADING_WHITE,  RASTERIZE_CULLING_NONE, ModelType::File,      CV_32F, CV_32S }
+));
+
+} // namespace ::
+} // namespace opencv_test