vendor: OpenCV 5.0.0 snapshot at 40738fb16ceddb5fb3fea747585f7ce6abb0605b

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);
+}
+
+}