diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp
index ae0fb130f0..a176da07c5 100644
--- a/modules/3d/include/opencv2/3d.hpp
+++ b/modules/3d/include/opencv2/3d.hpp
@@ -9,6 +9,9 @@
 #include "opencv2/core/types_c.h"
 
 #include "opencv2/3d/depth.hpp"
+#include "opencv2/3d/odometry.hpp"
+#include "opencv2/3d/odometry_frame.hpp"
+#include "opencv2/3d/odometry_settings.hpp"
 #include "opencv2/3d/volume.hpp"
 #include "opencv2/3d/ptcloud.hpp"
 
diff --git a/modules/3d/include/opencv2/3d/depth.hpp b/modules/3d/include/opencv2/3d/depth.hpp
index 32543cc020..735bfe4a7a 100644
--- a/modules/3d/include/opencv2/3d/depth.hpp
+++ b/modules/3d/include/opencv2/3d/depth.hpp
@@ -98,7 +98,7 @@ CV_EXPORTS_W void registerDepth(InputArray unregisteredCameraMatrix, InputArray
  * @param depth the depth image
  * @param in_K
  * @param in_points the list of xy coordinates
- * @param points3d the resulting 3d points
+ * @param points3d the resulting 3d points (point is represented by 4 chanels value [x, y, z, 0])
  */
 CV_EXPORTS_W void depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d);
 
@@ -107,7 +107,7 @@ CV_EXPORTS_W void depthTo3dSparse(InputArray depth, InputArray in_K, InputArray
  * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
  *              (as done with the Microsoft Kinect), otherwise, if given as CV_32F or CV_64F, it is assumed in meters)
  * @param K The calibration matrix
- * @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the
+ * @param points3d the resulting 3d points (point is represented by 4 chanels value [x, y, z, 0]). 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)
  */
@@ -166,331 +166,7 @@ CV_EXPORTS_W void findPlanes(InputArray points3d, InputArray normals, OutputArra
                              double sensor_error_c = 0,
                              RgbdPlaneMethod method = RGBD_PLANE_METHOD_DEFAULT);
 
-/** Object that contains a frame data that is possibly needed for the Odometry.
- * It's used for the efficiency (to pass precomputed/cached data of the frame that participates
- * in the Odometry processing several times).
- */
-struct CV_EXPORTS_W OdometryFrame
-{
-public:
-    /** These constants are used to set a type of cache which has to be prepared depending on the frame role:
-     * srcFrame or dstFrame (see compute method of the Odometry class). For the srcFrame and dstFrame different cache data may be required,
-     * some part of a cache may be common for both frame roles.
-     * @param CACHE_SRC The cache data for the srcFrame will be prepared.
-     * @param CACHE_DST The cache data for the dstFrame will be prepared.
-     * @param CACHE_ALL The cache data for both srcFrame and dstFrame roles will be computed.
-     * @param CACHE_DEPTH The frame will be generated from depth image
-     * @param CACHE_PTS The frame will be built from point cloud
-     */
-    enum OdometryFrameCacheType
-    {
-      CACHE_SRC = 1, CACHE_DST = 2, CACHE_ALL = CACHE_SRC + CACHE_DST
-    };
 
-    /** Indicates what pyramid is to access using get/setPyramid... methods:
-    * @param PYR_IMAGE The pyramid of RGB images
-    * @param PYR_DEPTH The pyramid of depth images
-    * @param PYR_MASK  The pyramid of masks
-    * @param PYR_CLOUD The pyramid of point clouds, produced from the pyramid of depths
-    * @param PYR_DIX   The pyramid of dI/dx derivative images
-    * @param PYR_DIY   The pyramid of dI/dy derivative images
-    * @param PYR_TEXMASK The pyramid of textured masks
-    * @param PYR_NORM  The pyramid of normals
-    * @param PYR_NORMMASK The pyramid of normals masks
-    */
-    enum OdometryFramePyramidType
-    {
-        PYR_IMAGE = 0, PYR_DEPTH = 1, PYR_MASK = 2, PYR_CLOUD = 3, PYR_DIX = 4, PYR_DIY = 5, PYR_TEXMASK = 6, PYR_NORM = 7, PYR_NORMMASK = 8,
-        N_PYRAMIDS
-    };
-
-    OdometryFrame() : ID(-1) { }
-    virtual ~OdometryFrame() { }
-
-    CV_WRAP static Ptr<OdometryFrame> create(InputArray image = noArray(), InputArray depth = noArray(),
-                                             InputArray  mask = noArray(), InputArray normals = noArray(), int ID = -1);
-
-    CV_WRAP virtual void setImage(InputArray  image) = 0;
-    CV_WRAP virtual void getImage(OutputArray image) = 0;
-    CV_WRAP virtual void setDepth(InputArray  depth) = 0;
-    CV_WRAP virtual void getDepth(OutputArray depth) = 0;
-    CV_WRAP virtual void setMask(InputArray  mask) = 0;
-    CV_WRAP virtual void getMask(OutputArray mask) = 0;
-    CV_WRAP virtual void setNormals(InputArray  normals) = 0;
-    CV_WRAP virtual void getNormals(OutputArray normals) = 0;
-
-    CV_WRAP virtual void setPyramidLevels(size_t nLevels) = 0;
-    CV_WRAP virtual size_t getPyramidLevels(OdometryFrame::OdometryFramePyramidType pyrType) = 0;
-
-    CV_WRAP virtual void setPyramidAt(InputArray  pyrImage, OdometryFrame::OdometryFramePyramidType pyrType, size_t level) = 0;
-    CV_WRAP virtual void getPyramidAt(OutputArray pyrImage, OdometryFrame::OdometryFramePyramidType pyrType, size_t level) = 0;
-
-    CV_PROP int ID;
-};
-
-
-/** Base class for computation of odometry.
- */
-class CV_EXPORTS_W Odometry
-{
-public:
-
-    /** A class of transformation*/
-    enum OdometryTransformType
-    {
-      ROTATION = 1, TRANSLATION = 2, RIGID_BODY_MOTION = 4
-    };
-
-    virtual ~Odometry() { }
-
-    /** Create a new Odometry object based on its name. Currently supported names are:
-    * "RgbdOdometry", "ICPOdometry", "RgbdICPOdometry", "FastICPOdometry".
-    * @param odometryType algorithm's name
-    */
-    CV_WRAP static Ptr<Odometry> createFromName(const std::string& odometryType);
-
-    /** Method to compute a transformation from the source frame to the destination one.
-     * Some odometry algorithms do not used some data of frames (eg. ICP does not use images).
-     * In such case corresponding arguments can be set as empty Mat.
-     * The method returns true if all internal computations were possible (e.g. there were enough correspondences,
-     * system of equations has a solution, etc) and resulting transformation satisfies some test if it's provided
-     * by the Odometry inheritor implementation (e.g. thresholds for maximum translation and rotation).
-     * @param srcImage Image data of the source frame (CV_8UC1)
-     * @param srcDepth Depth data of the source frame (CV_32FC1, in meters)
-     * @param srcMask Mask that sets which pixels have to be used from the source frame (CV_8UC1)
-     * @param dstImage Image data of the destination frame (CV_8UC1)
-     * @param dstDepth Depth data of the destination frame (CV_32FC1, in meters)
-     * @param dstMask Mask that sets which pixels have to be used from the destination frame (CV_8UC1)
-     * @param Rt Resulting transformation from the source frame to the destination one (rigid body motion):
-     dst_p = Rt * src_p, where dst_p is a homogeneous point in the destination frame and src_p is
-     homogeneous point in the source frame,
-     Rt is 4x4 matrix of CV_64FC1 type.
-     * @param initRt Initial transformation from the source frame to the destination one (optional)
-     */
-    CV_WRAP virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
-                                 InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const = 0;
-
-    /** One more method to compute a transformation from the source frame to the destination one.
-     * It is designed to save on computing the frame data (image pyramids, normals, etc.).
-     */
-    CV_WRAP_AS(compute2) virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
-                                              OutputArray Rt, InputArray initRt = noArray()) const = 0;
-
-    /** Prepare a cache for the frame. The function checks the precomputed/passed data (throws the error if this data
-     * does not satisfy) and computes all remaining cache data needed for the frame. Returned size is a resolution
-     * of the prepared frame.
-     * @param frame The odometry which will process the frame.
-     * @param cacheType The cache type: CACHE_SRC, CACHE_DST or CACHE_ALL.
-     */
-    CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const = 0;
-
-    /** Create odometry frame for current Odometry implementation
-     * @param image Image data of the frame (CV_8UC1)
-     * @param depth Depth data of the frame (CV_32FC1, in meters)
-     * @param mask  Mask that sets which pixels have to be used from the frame (CV_8UC1)
-    */
-    CV_WRAP virtual Ptr<OdometryFrame> makeOdometryFrame(InputArray image, InputArray depth, InputArray mask) const = 0;
-
-    CV_WRAP virtual void getCameraMatrix(OutputArray val) const = 0;
-    CV_WRAP virtual void setCameraMatrix(InputArray val) = 0;
-    CV_WRAP virtual Odometry::OdometryTransformType getTransformType() const = 0;
-    CV_WRAP virtual void setTransformType(Odometry::OdometryTransformType val) = 0;
-    CV_WRAP virtual void getIterationCounts(OutputArray val) const = 0;
-    CV_WRAP virtual void setIterationCounts(InputArray val) = 0;
-    /** For each pyramid level the pixels will be filtered out if they have gradient magnitude less than minGradientMagnitudes[level].
-    * Makes sense for RGB-based algorithms only.
-    */
-    CV_WRAP virtual void getMinGradientMagnitudes(OutputArray val) const = 0;
-    CV_WRAP virtual void setMinGradientMagnitudes(InputArray val) = 0;
-
-    /** Get max allowed translation in meters, default is 0.15 meters
-    Found delta transform is considered successful only if the translation is in given limits. */
-    CV_WRAP virtual double getMaxTranslation() const = 0;
-    /** Set max allowed translation in meters.
-    * Found delta transform is considered successful only if the translation is in given limits. */
-    CV_WRAP virtual void setMaxTranslation(double val) = 0;
-    /** Get max allowed rotation in degrees, default is 15 degrees.
-    * Found delta transform is considered successful only if the rotation is in given limits. */
-    CV_WRAP virtual double getMaxRotation() const = 0;
-    /** Set max allowed rotation in degrees.
-    * Found delta transform is considered successful only if the rotation is in given limits. */
-    CV_WRAP virtual void setMaxRotation(double val) = 0;
-};
-
-/** Odometry based on the paper "Real-Time Visual Odometry from Dense RGB-D Images",
- * F. Steinbucker, J. Strum, D. Cremers, ICCV, 2011.
- */
-class CV_EXPORTS_W RgbdOdometry: public Odometry
-{
-public:
-    RgbdOdometry() { }
-
-    /** Creates RgbdOdometry object
-     * @param cameraMatrix Camera matrix
-     * @param minDepth Pixels with depth less than minDepth will not be used (in meters)
-     * @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
-     * @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff (in meters)
-     * @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
-     * @param minGradientMagnitudes For each pyramid level the pixels will be filtered out, default is 10 per each level
-     *                              if they have gradient magnitude less than minGradientMagnitudes[level].
-     * @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart (from 0 to 1)
-     * @param transformType Class of transformation
-     */
-    CV_WRAP static Ptr<RgbdOdometry> create(InputArray cameraMatrix = noArray(),
-                                            float minDepth = 0.f,
-                                            float maxDepth = 4.f,
-                                            float maxDepthDiff = 0.07f,
-                                            InputArray iterCounts = noArray(),
-                                            InputArray minGradientMagnitudes = noArray(),
-                                            float maxPointsPart = 0.07f,
-                                            Odometry::OdometryTransformType transformType = Odometry::RIGID_BODY_MOTION);
-
-    CV_WRAP virtual double getMinDepth() const = 0;
-    CV_WRAP virtual void   setMinDepth(double val) = 0;
-    CV_WRAP virtual double getMaxDepth() const = 0;
-    CV_WRAP virtual void   setMaxDepth(double val) = 0;
-    CV_WRAP virtual double getMaxDepthDiff() const = 0;
-    CV_WRAP virtual void   setMaxDepthDiff(double val) = 0;
-    CV_WRAP virtual double getMaxPointsPart() const = 0;
-    CV_WRAP virtual void   setMaxPointsPart(double val) = 0;
-
-};
-
-/** Odometry based on the paper "KinectFusion: Real-Time Dense Surface Mapping and Tracking",
- * Richard A. Newcombe, Andrew Fitzgibbon, at al, SIGGRAPH, 2011.
- */
-class CV_EXPORTS_W ICPOdometry: public Odometry
-{
-public:
-    ICPOdometry() { }
-
-    /** Creates new ICPOdometry object
-     * @param cameraMatrix Camera matrix
-     * @param minDepth Pixels with depth less than minDepth will not be used (in meters)
-     * @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
-     * @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff (in meters)
-     * @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart (from 0 to 1)
-     * @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
-     * @param transformType Class of trasformation
-     */
-    CV_WRAP static Ptr<ICPOdometry> create(InputArray cameraMatrix = noArray(),
-                                           float minDepth = 0.f,
-                                           float maxDepth = 4.f,
-                                           float maxDepthDiff = 0.07f,
-                                           float maxPointsPart = 0.07f,
-                                           InputArray iterCounts = noArray(),
-                                           Odometry::OdometryTransformType transformType = Odometry::RIGID_BODY_MOTION);
-
-    CV_WRAP virtual double getMinDepth() const = 0;
-    CV_WRAP virtual void   setMinDepth(double val) = 0;
-    CV_WRAP virtual double getMaxDepth() const = 0;
-    CV_WRAP virtual void   setMaxDepth(double val) = 0;
-    CV_WRAP virtual double getMaxDepthDiff() const = 0;
-    CV_WRAP virtual void   setMaxDepthDiff(double val) = 0;
-    CV_WRAP virtual double getMaxPointsPart() const = 0;
-    CV_WRAP virtual void   setMaxPointsPart(double val) = 0;
-    CV_WRAP virtual Ptr<RgbdNormals> getNormalsComputer() const = 0;
-};
-
-/** Odometry that merges RgbdOdometry and ICPOdometry by minimize sum of their energy functions.
- */
-class CV_EXPORTS_W RgbdICPOdometry: public Odometry
-{
-public:
-    RgbdICPOdometry() { }
-
-    /** Creates RgbdICPOdometry object
-     * @param cameraMatrix Camera matrix
-     * @param minDepth Pixels with depth less than minDepth will not be used (in meters)
-     * @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
-     * @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff (in meters)
-     * @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart (from 0 to 1)
-     * @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
-     * @param minGradientMagnitudes For each pyramid level the pixels will be filtered out
-     *                              if they have gradient magnitude less than minGradientMagnitudes[level], default is 10 per each level
-     * @param transformType Class of trasformation
-     */
-    CV_WRAP static Ptr<RgbdICPOdometry> create(InputArray cameraMatrix = noArray(),
-                                               float minDepth = 0.f,
-                                               float maxDepth = 4.f,
-                                               float maxDepthDiff = 0.07f,
-                                               float maxPointsPart = 0.07f,
-                                               InputArray iterCounts = noArray(),
-                                               InputArray minGradientMagnitudes = noArray(),
-                                               Odometry::OdometryTransformType transformType = Odometry::RIGID_BODY_MOTION);
-
-    CV_WRAP virtual double getMinDepth() const = 0;
-    CV_WRAP virtual void   setMinDepth(double val) = 0;
-    CV_WRAP virtual double getMaxDepth() const = 0;
-    CV_WRAP virtual void   setMaxDepth(double val) = 0;
-    CV_WRAP virtual double getMaxDepthDiff() const = 0;
-    CV_WRAP virtual void   setMaxDepthDiff(double val) = 0;
-    CV_WRAP virtual double getMaxPointsPart() const = 0;
-    CV_WRAP virtual void   setMaxPointsPart(double val) = 0;
-
-    CV_WRAP virtual Ptr<RgbdNormals> getNormalsComputer() const = 0;
-};
-
-/** A faster version of ICPOdometry which is used in KinectFusion implementation
- * Partial list of differences:
- * - UMats are processed using OpenCL
- * - CPU version is written in universal intrinsics
- * - Filters points by angle
- * - Interpolates points and normals
- * - Doesn't use masks or min/max depth filtering
- * - Doesn't use random subsets of points
- * - Supports only Rt transform type
- * - Initial transform is not supported and always ignored
- * - Supports only 4-float vectors as input type
- */
-class CV_EXPORTS_W FastICPOdometry: public Odometry
-{
-public:
-    FastICPOdometry() { }
-
-    /** Creates FastICPOdometry object
-     * @param cameraMatrix Camera matrix
-     * @param maxDistDiff Correspondences between pixels of two given frames will be filtered out
-     *                    if their depth difference is larger than maxDepthDiff (in meters)
-     * @param angleThreshold Correspondence will be filtered out
-     *                     if an angle between their normals is bigger than threshold
-     * @param sigmaDepth Depth sigma in meters for bilateral smooth
-     * @param sigmaSpatial Spatial sigma in pixels for bilateral smooth
-     * @param kernelSize Kernel size in pixels for bilateral smooth
-     * @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
-     * @param depthFactor pre-scale per 1 meter for input values
-     * @param truncateThreshold Threshold for depth truncation in meters
-     *        All depth values beyond this threshold will be set to zero
-     */
-    CV_WRAP static Ptr<FastICPOdometry> create(InputArray cameraMatrix = noArray(),
-                                               float maxDistDiff = 0.07f,
-                                               float angleThreshold = (float)(30. * CV_PI / 180.),
-                                               float sigmaDepth = 0.04f,
-                                               float sigmaSpatial = 4.5f,
-                                               int kernelSize = 7,
-                                               InputArray iterCounts = noArray(),
-                                               float depthFactor = 1.f,
-                                               float truncateThreshold = 0.f);
-
-    CV_WRAP virtual double getMaxDistDiff() const = 0;
-    CV_WRAP virtual void   setMaxDistDiff(float val) = 0;
-    CV_WRAP virtual float  getAngleThreshold() const = 0;
-    CV_WRAP virtual void   setAngleThreshold(float f) = 0;
-    CV_WRAP virtual float  getSigmaDepth() const = 0;
-    CV_WRAP virtual void   setSigmaDepth(float f) = 0;
-    CV_WRAP virtual float  getSigmaSpatial() const = 0;
-    CV_WRAP virtual void   setSigmaSpatial(float f) = 0;
-    CV_WRAP virtual int    getKernelSize() const = 0;
-    CV_WRAP virtual void   setKernelSize(int f) = 0;
-    CV_WRAP virtual float  getDepthFactor() const = 0;
-    CV_WRAP virtual void   setDepthFactor(float depthFactor) = 0;
-    CV_WRAP virtual float  getTruncateThreshold() const = 0;
-    CV_WRAP virtual void   setTruncateThreshold(float truncateThreshold) = 0;
-
-};
 
 // TODO Depth interpolation
 // Curvature
diff --git a/modules/3d/include/opencv2/3d/detail/submap.hpp b/modules/3d/include/opencv2/3d/detail/submap.hpp
index 311e12c5d3..e50b4abe24 100644
--- a/modules/3d/include/opencv2/3d/detail/submap.hpp
+++ b/modules/3d/include/opencv2/3d/detail/submap.hpp
@@ -52,7 +52,7 @@ public:
     virtual ~Submap() = default;
 
     virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics, const int currframeId);
-    virtual void raycast(const cv::Affine3f& cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
+    virtual void raycast(const Odometry& icp, const cv::Affine3f& cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
                          OutputArray points = noArray(), OutputArray normals = noArray());
 
     virtual int getTotalAllocatedBlocks() const { return int(volume->getTotalVolumeUnits()); };
@@ -94,7 +94,8 @@ public:
     static constexpr int FRAME_VISIBILITY_THRESHOLD = 5;
 
     //! TODO: Add support for GPU arrays (UMat)
-    Ptr<OdometryFrame> frame;
+    OdometryFrame frame;
+    OdometryFrame renderFrame;
 
     std::shared_ptr<Volume> volume;
 };
@@ -109,17 +110,25 @@ void Submap<MatType>::integrate(InputArray _depth, float depthFactor, const cv::
 }
 
 template<typename MatType>
-void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
+void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
                               OutputArray points, OutputArray normals)
 {
     if (!points.needed() && !normals.needed())
     {
         MatType pts, nrm;
-        frame->getPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
-        frame->getPyramidAt(nrm, OdometryFrame::PYR_NORM, 0);
+
+        frame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
+        frame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
         volume->raycast(_cameraPose.matrix, intrinsics, frameSize, pts, nrm);
-        frame->setPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
-        frame->setPyramidAt(nrm, OdometryFrame::PYR_NORM,  0);
+        frame.setPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
+        frame.setPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM,  0);
+
+        renderFrame = frame;
+
+        Mat depth;
+        frame.getDepth(depth);
+        frame = icp.createOdometryFrame();
+        frame.setDepth(depth);
     }
     else
     {
@@ -195,7 +204,7 @@ public:
     Ptr<SubmapT> getCurrentSubmap(void) const;
 
     int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose);
-    bool updateMap(int _frameId, Ptr<OdometryFrame> _frame);
+    bool updateMap(int frameId, const OdometryFrame& frame);
 
     bool addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID);
 
@@ -418,7 +427,7 @@ bool SubmapManager<MatType>::addEdgeToCurrentSubmap(const int currentSubmapID, c
 }
 
 template<typename MatType>
-bool SubmapManager<MatType>::updateMap(int _frameId, Ptr<OdometryFrame> _frame)
+bool SubmapManager<MatType>::updateMap(int _frameId, const OdometryFrame& _frame)
 {
     bool mapUpdated = false;
     int changedCurrentMapId = -1;
diff --git a/modules/3d/include/opencv2/3d/odometry.hpp b/modules/3d/include/opencv2/3d/odometry.hpp
new file mode 100644
index 0000000000..61e02f45c8
--- /dev/null
+++ b/modules/3d/include/opencv2/3d/odometry.hpp
@@ -0,0 +1,81 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#ifndef OPENCV_3D_ODOMETRY_HPP
+#define OPENCV_3D_ODOMETRY_HPP
+
+#include <opencv2/core.hpp>
+
+#include "odometry_frame.hpp"
+#include "odometry_settings.hpp"
+
+namespace cv
+{
+
+/** These constants are used to set a type of data which odometry will use
+* @param DEPTH     only depth data
+* @param RGB       only rgb image
+* @param RGB_DEPTH only depth and rgb data simultaneously
+*/
+enum class OdometryType
+{
+    DEPTH     = 0,
+    RGB       = 1,
+    RGB_DEPTH = 2
+};
+
+/** These constants are used to set the speed and accuracy of odometry
+* @param COMMON only accurate but not so fast
+* @param FAST   only less accurate but faster
+*/
+enum class OdometryAlgoType
+{
+    COMMON = 0,
+    FAST   = 1
+};
+
+class CV_EXPORTS_W Odometry
+{
+public:
+    Odometry();
+    Odometry(OdometryType otype, const OdometrySettings settings, OdometryAlgoType algtype);
+    ~Odometry();
+
+    /** Create new odometry frame
+     * The Type (Mat or UMat) depends on odometry type
+     */
+    OdometryFrame createOdometryFrame() const;
+
+    // Deprecated
+    OdometryFrame createOdometryFrame(OdometryFrameStoreType matType) const;
+
+    /** Prepare frame for odometry calculation
+     * @param frame odometry prepare this frame as src frame and dst frame simultaneously
+     */
+    void prepareFrame(OdometryFrame& frame);
+
+    /** Prepare frame for odometry calculation
+     * @param srcFrame frame will be prepared as src frame ("original" image)
+     * @param dstFrame frame will be prepared as dsr frame ("rotated" image)
+     */
+    void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame);
+
+    /** Prepare frame for odometry calculation
+     * @param srcFrame src frame ("original" image)
+     * @param dstFrame dsr frame ("rotated" image)
+     * @param Rt Rigid transformation, which will be calculated, in form:
+     * { R_11 R_12 R_13 t_1
+     *   R_21 R_22 R_23 t_2
+     *   R_31 R_32 R_33 t_3
+     *   0    0    0    1  }
+     */
+    bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt);
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
+};
+
+}
+#endif //OPENCV_3D_ODOMETRY_HPP
diff --git a/modules/3d/include/opencv2/3d/odometry_frame.hpp b/modules/3d/include/opencv2/3d/odometry_frame.hpp
new file mode 100644
index 0000000000..509386b347
--- /dev/null
+++ b/modules/3d/include/opencv2/3d/odometry_frame.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 OPENCV_3D_ODOMETRY_FRAME_HPP
+#define OPENCV_3D_ODOMETRY_FRAME_HPP
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+/** Indicates what pyramid is to access using get/setPyramid... methods:
+* @param PYR_IMAGE The pyramid of RGB images
+* @param PYR_DEPTH The pyramid of depth images
+* @param PYR_MASK  The pyramid of masks
+* @param PYR_CLOUD The pyramid of point clouds, produced from the pyramid of depths
+* @param PYR_DIX   The pyramid of dI/dx derivative images
+* @param PYR_DIY   The pyramid of dI/dy derivative images
+* @param PYR_TEXMASK The pyramid of textured masks
+* @param PYR_NORM  The pyramid of normals
+* @param PYR_NORMMASK The pyramid of normals masks
+*/
+
+enum OdometryFramePyramidType
+{
+    PYR_IMAGE = 0,
+    PYR_DEPTH = 1,
+    PYR_MASK = 2,
+    PYR_CLOUD = 3,
+    PYR_DIX = 4,
+    PYR_DIY = 5,
+    PYR_TEXMASK = 6,
+    PYR_NORM = 7,
+    PYR_NORMMASK = 8,
+    N_PYRAMIDS
+};
+
+enum class OdometryFrameStoreType
+{
+    MAT  = 0,
+    UMAT = 1
+};
+
+class CV_EXPORTS_W OdometryFrame
+{
+public:
+    OdometryFrame();
+    OdometryFrame(OdometryFrameStoreType matType);
+    ~OdometryFrame() {};
+    void setImage(InputArray  image);
+    void getImage(OutputArray image) const;
+    void getGrayImage(OutputArray image) const;
+    void setDepth(InputArray  depth);
+    void getDepth(OutputArray depth) const;
+    void setMask(InputArray  mask);
+    void getMask(OutputArray mask) const;
+    void setNormals(InputArray  normals);
+    void getNormals(OutputArray normals) const;
+    void setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype);
+    void setPyramidLevels(size_t _nLevels);
+    size_t getPyramidLevels(OdometryFramePyramidType oftype) const;
+    void setPyramidAt(InputArray  img, OdometryFramePyramidType pyrType, size_t level);
+    void getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const;
+
+    class Impl;
+private:
+    Ptr<Impl> impl;
+};
+}
+#endif // !OPENCV_3D_ODOMETRY_FRAME_HPP
diff --git a/modules/3d/include/opencv2/3d/odometry_settings.hpp b/modules/3d/include/opencv2/3d/odometry_settings.hpp
new file mode 100644
index 0000000000..9b38d7b9f5
--- /dev/null
+++ b/modules/3d/include/opencv2/3d/odometry_settings.hpp
@@ -0,0 +1,58 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#ifndef OPENCV_3D_ODOMETRY_SETTINGS_HPP
+#define OPENCV_3D_ODOMETRY_SETTINGS_HPP
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+
+class CV_EXPORTS_W OdometrySettings
+{
+public:
+    OdometrySettings();
+    ~OdometrySettings() {};
+    void setCameraMatrix(InputArray val);
+    void getCameraMatrix(OutputArray val) const;
+    void setIterCounts(InputArray val);
+    void getIterCounts(OutputArray val) const;
+
+    void  setMinDepth(float val);
+    float getMinDepth() const;
+    void  setMaxDepth(float val);
+    float getMaxDepth() const;
+    void  setMaxDepthDiff(float val);
+    float getMaxDepthDiff() const;
+    void  setMaxPointsPart(float val);
+    float getMaxPointsPart() const;
+
+    void setSobelSize(int val);
+    int  getSobelSize() const;
+    void   setSobelScale(double val);
+    double getSobelScale() const;
+    void setNormalWinSize(int val);
+    int  getNormalWinSize() const;
+
+    void  setAngleThreshold(float val);
+    float getAngleThreshold() const;
+    void  setMaxTranslation(float val);
+    float getMaxTranslation() const;
+    void  setMaxRotation(float val);
+    float getMaxRotation() const;
+
+    void  setMinGradientMagnitude(float val);
+    float getMinGradientMagnitude() const;
+    void setMinGradientMagnitudes(InputArray val);
+    void getMinGradientMagnitudes(OutputArray val) const;
+
+    class Impl;
+
+private:
+    Ptr<Impl> impl;
+};
+
+}
+#endif //OPENCV_3D_ODOMETRY_SETTINGS_HPP
diff --git a/modules/3d/samples/odometry_evaluation.cpp b/modules/3d/samples/odometry_evaluation.cpp
index 51cd81d4e7..7b1776cde6 100644
--- a/modules/3d/samples/odometry_evaluation.cpp
+++ b/modules/3d/samples/odometry_evaluation.cpp
@@ -80,8 +80,14 @@ int main(int argc, char** argv)
     if( !file.is_open() )
         return -1;
 
-    char dlmrt = '/';
-    size_t pos = filename.rfind(dlmrt);
+    char dlmrt1 = '/';
+    char dlmrt2 = '\\';
+
+    size_t pos1 = filename.rfind(dlmrt1);
+    size_t pos2 = filename.rfind(dlmrt2);
+    size_t pos = pos1 < pos2 ? pos1 : pos2;
+    char dlmrt = pos1 < pos2 ? dlmrt1 : dlmrt2;
+
     string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;
 
     const int timestampLength = 17;
@@ -104,15 +110,26 @@ int main(int argc, char** argv)
         cameraMatrix.at<float>(1,2) = cy;
     }
 
-    Ptr<OdometryFrame> frame_prev = OdometryFrame::create(),
-                       frame_curr = OdometryFrame::create();
-    Ptr<Odometry> odometry = Odometry::createFromName(string(argv[3]) + "Odometry");
-    if(odometry.empty())
+    OdometrySettings ods;
+    ods.setCameraMatrix(cameraMatrix);
+    Odometry odometry;
+    String odname = string(argv[3]);
+    if (odname == "Rgbd")
+        odometry = Odometry(OdometryType::RGB, ods, OdometryAlgoType::COMMON);
+    else if (odname == "ICP")
+        odometry = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::COMMON);
+    else if (odname == "RgbdICP")
+        odometry = Odometry(OdometryType::RGB_DEPTH, ods, OdometryAlgoType::COMMON);
+    else if (odname == "FastICP")
+        odometry = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
+    else
     {
-        cout << "Can not create Odometry algorithm. Check the passed odometry name." << endl;
+        std::cout << "Can not create Odometry algorithm. Check the passed odometry name." << std::endl;
         return -1;
     }
-    odometry->setCameraMatrix(cameraMatrix);
+
+    OdometryFrame frame_prev = odometry.createOdometryFrame(),
+                  frame_curr = odometry.createOdometryFrame();
 
     TickMeter gtm;
     int count = 0;
@@ -141,8 +158,6 @@ int main(int argc, char** argv)
             CV_Assert(!depth.empty());
             CV_Assert(depth.type() == CV_16UC1);
 
-            cout << i << " " << rgbFilename << " " << depthFilename << endl;
-
             // scale depth
             Mat depth_flt;
             depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
@@ -167,8 +182,8 @@ int main(int argc, char** argv)
         {
             Mat gray;
             cvtColor(image, gray, COLOR_BGR2GRAY);
-            frame_curr->setImage(gray);
-            frame_curr->setDepth(depth);
+            frame_curr.setImage(gray);
+            frame_curr.setDepth(depth);
 
             Mat Rt;
             if(!Rts.empty())
@@ -176,7 +191,8 @@ int main(int argc, char** argv)
                 TickMeter tm;
                 tm.start();
                 gtm.start();
-                bool res = odometry->compute(frame_curr, frame_prev, Rt);
+                odometry.prepareFrames(frame_curr, frame_prev);
+                bool res = odometry.compute(frame_curr, frame_prev, Rt);
                 gtm.stop();
                 tm.stop();
                 count++;
@@ -197,9 +213,11 @@ int main(int argc, char** argv)
                 Rts.push_back( prevRt * Rt );
             }
 
-            if (!frame_prev.empty())
-                frame_prev.release();
-            std::swap(frame_prev, frame_curr);
+            //if (!frame_prev.empty())
+            //    frame_prev.release();
+            frame_prev = frame_curr;
+            frame_curr = odometry.createOdometryFrame();
+            //std::swap(frame_prev, frame_curr);
         }
     }
 
diff --git a/modules/3d/src/rgbd/depth_to_3d.cpp b/modules/3d/src/rgbd/depth_to_3d.cpp
index 79aea236f4..ec7545362f 100644
--- a/modules/3d/src/rgbd/depth_to_3d.cpp
+++ b/modules/3d/src/rgbd/depth_to_3d.cpp
@@ -36,7 +36,7 @@ static void depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const
     float cx = K(0, 2);
     float cy = K(1, 2);
 
-    std::vector<cv::Mat> coordinates(3);
+    std::vector<cv::Mat> coordinates(4);
 
     coordinates[0] = (u_mat - cx) / fx;
 
@@ -46,6 +46,7 @@ static void depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const
     coordinates[0] = coordinates[0].mul(z_mat);
     coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy);
     coordinates[2] = z_mat;
+    coordinates[3] = 0;
     cv::merge(coordinates, points3d);
 }
 
@@ -86,7 +87,7 @@ static void depthTo3dMask(const cv::Mat& depth, const cv::Mat& K, const cv::Mat&
     z_mat.resize(n_points);
 
     depthTo3d_from_uvz(K, u_mat, v_mat, z_mat, points3d);
-    points3d = points3d.reshape(3, 1);
+    points3d = points3d.reshape(4, 1);
 }
 
 /**
@@ -120,7 +121,7 @@ void depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_<T>& K, cv::Mat& poi
     y_cache_ptr = y_cache[0];
     for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
     {
-        cv::Vec<T, 3>* point = points3d.ptr<cv::Vec<T, 3> >(y);
+        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)
@@ -129,6 +130,7 @@ void depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_<T>& K, cv::Mat& poi
             (*point)[0] = (*x_cache_ptr) * z;
             (*point)[1] = (*y_cache_ptr) * z;
             (*point)[2] = z;
+            (*point)[3] = 0;
         }
     }
 }
@@ -170,7 +172,7 @@ void depthTo3dSparse(InputArray depth_in, InputArray K_in, InputArray points_in,
     std::vector<cv::Mat> channels(2);
     cv::split(points_float, channels);
 
-    points3d_out.create(channels[0].rows, channels[0].cols, CV_32FC3);
+    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);
 }
@@ -200,12 +202,12 @@ void depthTo3d(InputArray depth_in, InputArray K_in, OutputArray points3d_out, I
     {
         cv::Mat points3d;
         depthTo3dMask(depth, K_new, mask, points3d);
-        points3d_out.create(points3d.size(), CV_MAKETYPE(K_new.depth(), 3));
+        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(), 3));
+        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);
diff --git a/modules/3d/src/rgbd/fast_icp.cpp b/modules/3d/src/rgbd/fast_icp.cpp
deleted file mode 100644
index cc9966977e..0000000000
--- a/modules/3d/src/rgbd/fast_icp.cpp
+++ /dev/null
@@ -1,656 +0,0 @@
-// 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 "fast_icp.hpp"
-#include "opencl_kernels_3d.hpp"
-
-using namespace std;
-
-namespace cv {
-namespace kinfu {
-
-enum
-{
-    UTSIZE = 27
-};
-
-ICP::ICP(const Matx33f _intrinsics, const std::vector<int>& _iterations, float _angleThreshold, float _distanceThreshold) :
-    iterations(_iterations), angleThreshold(_angleThreshold), distanceThreshold(_distanceThreshold),
-    intrinsics(_intrinsics)
-{ }
-
-class ICPImpl : public ICP
-{
-public:
-    ICPImpl(const cv::Matx33f _intrinsics, const std::vector<int> &_iterations, float _angleThreshold, float _distanceThreshold);
-
-    virtual bool estimateTransform(cv::Affine3f& transform,
-                                   InputArray oldPoints, InputArray oldNormals,
-                                   InputArray newPoints, InputArray newNormals
-                                   ) const override;
-    template < typename T >
-    bool estimateTransformT(cv::Affine3f& transform,
-                            const vector<T>& oldPoints, const vector<T>& oldNormals,
-                            const vector<T>& newPoints, const vector<T>& newNormals
-                            ) const;
-
-    virtual ~ICPImpl() { }
-
-    template < typename T >
-    void getAb(const T& oldPts, const T& oldNrm, const T& newPts, const T& newNrm,
-               cv::Affine3f pose, int level, cv::Matx66f& A, cv::Vec6f& b) const;
-
-private:
-
-    mutable vector<UMat> groupedSumBuffers;
-
-};
-
-ICPImpl::ICPImpl(const Matx33f _intrinsics, const std::vector<int> &_iterations, float _angleThreshold, float _distanceThreshold) :
-    ICP(_intrinsics, _iterations, _angleThreshold, _distanceThreshold),
-    groupedSumBuffers(_iterations.size())
-{ }
-
-
-bool ICPImpl::estimateTransform(cv::Affine3f& transform,
-                                InputArray _oldPoints, InputArray _oldNormals,
-                                InputArray _newPoints, InputArray _newNormals
-                                ) const
-{
-    CV_TRACE_FUNCTION();
-
-    CV_Assert(_oldPoints.size() == _oldNormals.size());
-    CV_Assert(_newPoints.size() == _newNormals.size());
-    CV_Assert(_oldPoints.size() == _newPoints.size());
-
-#ifdef HAVE_OPENCL
-    if(cv::ocl::isOpenCLActivated() &&
-       _oldPoints.isUMatVector() && _oldNormals.isUMatVector() &&
-       _newPoints.isUMatVector() && _newNormals.isUMatVector())
-    {
-        std::vector<UMat> op, np, on, nn;
-        _oldPoints.getUMatVector(op);
-        _newPoints.getUMatVector(np);
-        _oldNormals.getUMatVector(on);
-        _newNormals.getUMatVector(nn);
-        return estimateTransformT<UMat>(transform, op, on, np, nn);
-    }
-#endif
-
-    std::vector<Mat> op, on, np, nn;
-    _oldPoints.getMatVector(op);
-    _newPoints.getMatVector(np);
-    _oldNormals.getMatVector(on);
-    _newNormals.getMatVector(nn);
-    return estimateTransformT<Mat>(transform, op, on, np, nn);
-}
-
-template < typename T >
-bool ICPImpl::estimateTransformT(cv::Affine3f& transform,
-                                 const vector<T>& oldPoints, const vector<T>& oldNormals,
-                                 const vector<T>& newPoints, const vector<T>& newNormals
-                                 ) const
-{
-    CV_TRACE_FUNCTION();
-
-    transform = Affine3f::Identity();
-    for(size_t l = 0; l < iterations.size(); l++)
-    {
-        size_t level = iterations.size() - 1 - l;
-
-        const T& oldPts = oldPoints [level], newPts = newPoints [level];
-        const T& oldNrm = oldNormals[level], newNrm = newNormals[level];
-
-        for(int iter = 0; iter < iterations[level]; iter++)
-        {
-            Matx66f A;
-            Vec6f b;
-
-            getAb(oldPts, oldNrm, newPts, newNrm, transform, (int)level, A, b);
-
-            double det = cv::determinant(A);
-
-            if (abs (det) < 1e-15 || cvIsNaN(det))
-                return false;
-
-            Vec6f x;
-            // theoretically, any method of solving is applicable
-            // since there are usual least square matrices
-            solve(A, b, x, DECOMP_SVD);
-            Affine3f tinc(Vec3f(x.val), Vec3f(x.val+3));
-            transform = tinc * transform;
-        }
-    }
-
-    return true;
-}
-
-
-///////// CPU implementation /////////
-
-// 1 any coord to check is enough since we know the generation
-
-
-#if USE_INTRINSICS
-static inline bool fastCheck(const v_float32x4& p0, const v_float32x4& p1)
-{
-    float check = (p0.get0() + p1.get0());
-    return !cvIsNaN(check);
-}
-
-static inline void getCrossPerm(const v_float32x4& a, v_float32x4& yzx, v_float32x4& zxy)
-{
-    v_uint32x4 aa = v_reinterpret_as_u32(a);
-    v_uint32x4 yz00 = v_extract<1>(aa, v_setzero_u32());
-    v_uint32x4 x0y0, tmp;
-    v_zip(aa, v_setzero_u32(), x0y0, tmp);
-    v_uint32x4 yzx0 = v_combine_low(yz00, x0y0);
-    v_uint32x4 y000 = v_extract<2>(x0y0, v_setzero_u32());
-    v_uint32x4 zx00 = v_extract<1>(yzx0, v_setzero_u32());
-    zxy = v_reinterpret_as_f32(v_combine_low(zx00, y000));
-    yzx = v_reinterpret_as_f32(yzx0);
-}
-
-static inline v_float32x4 crossProduct(const v_float32x4& a, const v_float32x4& b)
-{
-    v_float32x4 ayzx, azxy, byzx, bzxy;
-    getCrossPerm(a, ayzx, azxy);
-    getCrossPerm(b, byzx, bzxy);
-    return ayzx*bzxy - azxy*byzx;
-}
-#else
-static inline bool fastCheck(const Point3f& p)
-{
-    return !cvIsNaN(p.x);
-}
-
-#endif
-
-typedef Matx<float, 6, 7> ABtype;
-
-struct GetAbInvoker : ParallelLoopBody
-{
-    GetAbInvoker(ABtype& _globalAb, Mutex& _mtx,
-                 const Points& _oldPts, const Normals& _oldNrm, const Points& _newPts, const Normals& _newNrm,
-                 Affine3f _pose, Intr::Projector _proj, float _sqDistanceThresh, float _minCos) :
-        ParallelLoopBody(),
-        globalSumAb(_globalAb), mtx(_mtx),
-        oldPts(_oldPts), oldNrm(_oldNrm), newPts(_newPts), newNrm(_newNrm), pose(_pose),
-        proj(_proj), sqDistanceThresh(_sqDistanceThresh), minCos(_minCos)
-    { }
-
-    virtual void operator ()(const Range& range) const override
-    {
-#if USE_INTRINSICS
-        CV_Assert(ptype::channels == 4);
-
-        const size_t utBufferSize = 9;
-        float CV_DECL_ALIGNED(16) upperTriangle[utBufferSize*4];
-        for(size_t i = 0; i < utBufferSize*4; i++)
-            upperTriangle[i] = 0;
-        // how values are kept in upperTriangle
-        const int NA = 0;
-        const size_t utPos[] =
-        {
-           0,  1,  2,  4,  5,  6,  3,
-          NA,  9, 10, 12, 13, 14, 11,
-          NA, NA, 18, 20, 21, 22, 19,
-          NA, NA, NA, 24, 28, 30, 32,
-          NA, NA, NA, NA, 25, 29, 33,
-          NA, NA, NA, NA, NA, 26, 34
-        };
-
-        const float (&pm)[16] = pose.matrix.val;
-        v_float32x4 poseRot0(pm[0], pm[4], pm[ 8], 0);
-        v_float32x4 poseRot1(pm[1], pm[5], pm[ 9], 0);
-        v_float32x4 poseRot2(pm[2], pm[6], pm[10], 0);
-        v_float32x4 poseTrans(pm[3], pm[7], pm[11], 0);
-
-        v_float32x4 vfxy(proj.fx, proj.fy, 0, 0), vcxy(proj.cx, proj.cy, 0, 0);
-        v_float32x4 vframe((float)(oldPts.cols - 1), (float)(oldPts.rows - 1), 1.f, 1.f);
-
-        float sqThresh = sqDistanceThresh;
-        float cosThresh = minCos;
-
-        for(int y = range.start; y < range.end; y++)
-        {
-            const CV_DECL_ALIGNED(16) float* newPtsRow = (const float*)newPts[y];
-            const CV_DECL_ALIGNED(16) float* newNrmRow = (const float*)newNrm[y];
-
-            for(int x = 0; x < newPts.cols; x++)
-            {
-                v_float32x4 newP = v_load_aligned(newPtsRow + x*4);
-                v_float32x4 newN = v_load_aligned(newNrmRow + x*4);
-
-                if(!fastCheck(newP, newN))
-                    continue;
-
-                //transform to old coord system
-                newP = v_matmuladd(newP, poseRot0, poseRot1, poseRot2, poseTrans);
-                newN = v_matmuladd(newN, poseRot0, poseRot1, poseRot2, v_setzero_f32());
-
-                //find correspondence by projecting the point
-                v_float32x4 oldCoords;
-                float pz = (v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(newP))).get0());
-                // x, y, 0, 0
-                oldCoords = v_muladd(newP/v_setall_f32(pz), vfxy, vcxy);
-
-                if(!v_check_all((oldCoords >= v_setzero_f32()) & (oldCoords < vframe)))
-                    continue;
-
-                // bilinearly interpolate oldPts and oldNrm under oldCoords point
-                v_float32x4 oldP;
-                v_float32x4 oldN;
-                {
-                    v_int32x4 ixy = v_floor(oldCoords);
-                    v_float32x4 txy = oldCoords - v_cvt_f32(ixy);
-                    int xi = ixy.get0();
-                    int yi = v_rotate_right<1>(ixy).get0();
-                    v_float32x4 tx = v_setall_f32(txy.get0());
-                    txy = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(txy)));
-                    v_float32x4 ty = v_setall_f32(txy.get0());
-
-                    const float* prow0 = (const float*)oldPts[yi+0];
-                    const float* prow1 = (const float*)oldPts[yi+1];
-
-                    v_float32x4 p00 = v_load(prow0 + (xi+0)*4);
-                    v_float32x4 p01 = v_load(prow0 + (xi+1)*4);
-                    v_float32x4 p10 = v_load(prow1 + (xi+0)*4);
-                    v_float32x4 p11 = v_load(prow1 + (xi+1)*4);
-
-                    // do not fix missing data
-                    // NaN check is done later
-
-                    const float* nrow0 = (const float*)oldNrm[yi+0];
-                    const float* nrow1 = (const float*)oldNrm[yi+1];
-
-                    v_float32x4 n00 = v_load(nrow0 + (xi+0)*4);
-                    v_float32x4 n01 = v_load(nrow0 + (xi+1)*4);
-                    v_float32x4 n10 = v_load(nrow1 + (xi+0)*4);
-                    v_float32x4 n11 = v_load(nrow1 + (xi+1)*4);
-
-                    // NaN check is done later
-
-                    v_float32x4 p0 = p00 + tx*(p01 - p00);
-                    v_float32x4 p1 = p10 + tx*(p11 - p10);
-                    oldP = p0 + ty*(p1 - p0);
-
-                    v_float32x4 n0 = n00 + tx*(n01 - n00);
-                    v_float32x4 n1 = n10 + tx*(n11 - n10);
-                    oldN = n0 + ty*(n1 - n0);
-                }
-
-                bool oldPNcheck = fastCheck(oldP, oldN);
-
-                //filter by distance
-                v_float32x4 diff = newP - oldP;
-                bool distCheck = !(v_reduce_sum(diff*diff) > sqThresh);
-
-                //filter by angle
-                bool angleCheck = !(abs(v_reduce_sum(newN*oldN)) < cosThresh);
-
-                if(!(oldPNcheck && distCheck && angleCheck))
-                    continue;
-
-                // build point-wise vector ab = [ A | b ]
-                v_float32x4 VxNv = crossProduct(newP, oldN);
-                Point3f VxN;
-                VxN.x = VxNv.get0();
-                VxN.y = v_reinterpret_as_f32(v_extract<1>(v_reinterpret_as_u32(VxNv), v_setzero_u32())).get0();
-                VxN.z = v_reinterpret_as_f32(v_extract<2>(v_reinterpret_as_u32(VxNv), v_setzero_u32())).get0();
-
-                float dotp = -v_reduce_sum(oldN*diff);
-
-                // build point-wise upper-triangle matrix [ab^T * ab] w/o last row
-                // which is [A^T*A | A^T*b]
-                // and gather sum
-
-                v_float32x4 vd = VxNv | v_float32x4(0, 0, 0, dotp);
-                v_float32x4 n = oldN;
-                v_float32x4 nyzx;
-                {
-                    v_uint32x4 aa = v_reinterpret_as_u32(n);
-                    v_uint32x4 yz00 = v_extract<1>(aa, v_setzero_u32());
-                    v_uint32x4 x0y0, tmp;
-                    v_zip(aa, v_setzero_u32(), x0y0, tmp);
-                    nyzx = v_reinterpret_as_f32(v_combine_low(yz00, x0y0));
-                }
-
-                v_float32x4 vutg[utBufferSize];
-                for(size_t i = 0; i < utBufferSize; i++)
-                    vutg[i] = v_load_aligned(upperTriangle + i*4);
-
-                int p = 0;
-                v_float32x4 v;
-                // vx * vd, vx * n
-                v = v_setall_f32(VxN.x);
-                v_store_aligned(upperTriangle + p*4, v_muladd(v, vd, vutg[p])); p++;
-                v_store_aligned(upperTriangle + p*4, v_muladd(v,  n, vutg[p])); p++;
-                // vy * vd, vy * n
-                v = v_setall_f32(VxN.y);
-                v_store_aligned(upperTriangle + p*4, v_muladd(v, vd, vutg[p])); p++;
-                v_store_aligned(upperTriangle + p*4, v_muladd(v,  n, vutg[p])); p++;
-                // vz * vd, vz * n
-                v = v_setall_f32(VxN.z);
-                v_store_aligned(upperTriangle + p*4, v_muladd(v, vd, vutg[p])); p++;
-                v_store_aligned(upperTriangle + p*4, v_muladd(v,  n, vutg[p])); p++;
-                // nx^2, ny^2, nz^2
-                v_store_aligned(upperTriangle + p*4, v_muladd(n, n, vutg[p])); p++;
-                // nx*ny, ny*nz, nx*nz
-                v_store_aligned(upperTriangle + p*4, v_muladd(n, nyzx, vutg[p])); p++;
-                // nx*d, ny*d, nz*d
-                v = v_setall_f32(dotp);
-                v_store_aligned(upperTriangle + p*4, v_muladd(n, v, vutg[p])); p++;
-            }
-        }
-
-        ABtype sumAB = ABtype::zeros();
-        for(int i = 0; i < 6; i++)
-        {
-            for(int j = i; j < 7; j++)
-            {
-                size_t p = utPos[i*7+j];
-                sumAB(i, j) = upperTriangle[p];
-            }
-        }
-
-#else
-
-        float upperTriangle[UTSIZE];
-        for(int i = 0; i < UTSIZE; i++)
-            upperTriangle[i] = 0;
-
-        for(int y = range.start; y < range.end; y++)
-        {
-            const ptype* newPtsRow = newPts[y];
-            const ptype* newNrmRow = newNrm[y];
-
-            for(int x = 0; x < newPts.cols; x++)
-            {
-                Point3f newP = fromPtype(newPtsRow[x]);
-                Point3f newN = fromPtype(newNrmRow[x]);
-
-                Point3f oldP(nan3), oldN(nan3);
-
-                if(!(fastCheck(newP) && fastCheck(newN)))
-                    continue;
-
-                //transform to old coord system
-                newP = pose * newP;
-                newN = pose.rotation() * newN;
-
-                //find correspondence by projecting the point
-                Point2f oldCoords = proj(newP);
-                if(!(oldCoords.x >= 0 && oldCoords.x < oldPts.cols - 1 &&
-                     oldCoords.y >= 0 && oldCoords.y < oldPts.rows - 1))
-                    continue;
-
-                // bilinearly interpolate oldPts and oldNrm under oldCoords point
-                int xi = cvFloor(oldCoords.x), yi = cvFloor(oldCoords.y);
-                float tx  = oldCoords.x - xi, ty = oldCoords.y - yi;
-
-                const ptype* prow0 = oldPts[yi+0];
-                const ptype* prow1 = oldPts[yi+1];
-
-                Point3f p00 = fromPtype(prow0[xi+0]);
-                Point3f p01 = fromPtype(prow0[xi+1]);
-                Point3f p10 = fromPtype(prow1[xi+0]);
-                Point3f p11 = fromPtype(prow1[xi+1]);
-
-                //do not fix missing data
-                if(!(fastCheck(p00) && fastCheck(p01) &&
-                     fastCheck(p10) && fastCheck(p11)))
-                    continue;
-
-                const ptype* nrow0 = oldNrm[yi+0];
-                const ptype* nrow1 = oldNrm[yi+1];
-
-                Point3f n00 = fromPtype(nrow0[xi+0]);
-                Point3f n01 = fromPtype(nrow0[xi+1]);
-                Point3f n10 = fromPtype(nrow1[xi+0]);
-                Point3f n11 = fromPtype(nrow1[xi+1]);
-
-                if(!(fastCheck(n00) && fastCheck(n01) &&
-                     fastCheck(n10) && fastCheck(n11)))
-                    continue;
-
-                Point3f p0 = p00 + tx*(p01 - p00);
-                Point3f p1 = p10 + tx*(p11 - p10);
-                oldP = p0 + ty*(p1 - p0);
-
-                Point3f n0 = n00 + tx*(n01 - n00);
-                Point3f n1 = n10 + tx*(n11 - n10);
-                oldN = n0 + ty*(n1 - n0);
-
-                if(!(fastCheck(oldP) && fastCheck(oldN)))
-                    continue;
-
-                //filter by distance
-                Point3f diff = newP - oldP;
-                if(diff.dot(diff) > sqDistanceThresh)
-                {
-                    continue;
-                }
-
-                //filter by angle
-                if(abs(newN.dot(oldN)) < minCos)
-                {
-                    continue;
-                }
-
-                // build point-wise vector ab = [ A | b ]
-
-                //try to optimize
-                Point3f VxN = newP.cross(oldN);
-                float ab[7] = {VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, oldN.dot(-diff)};
-                // build point-wise upper-triangle matrix [ab^T * ab] w/o last row
-                // which is [A^T*A | A^T*b]
-                // and gather sum
-                int pos = 0;
-                for(int i = 0; i < 6; i++)
-                {
-                    for(int j = i; j < 7; j++)
-                    {
-                        upperTriangle[pos++] += ab[i]*ab[j];
-                    }
-                }
-            }
-        }
-
-        ABtype sumAB = ABtype::zeros();
-        int pos = 0;
-        for(int i = 0; i < 6; i++)
-        {
-            for(int j = i; j < 7; j++)
-            {
-                sumAB(i, j) = upperTriangle[pos++];
-            }
-        }
-#endif
-
-        AutoLock al(mtx);
-        globalSumAb += sumAB;
-    }
-
-    ABtype& globalSumAb;
-    Mutex& mtx;
-    const Points& oldPts;
-    const Normals& oldNrm;
-    const Points& newPts;
-    const Normals& newNrm;
-    Affine3f pose;
-    const Intr::Projector proj;
-    float sqDistanceThresh;
-    float minCos;
-};
-
-
-template <>
-void ICPImpl::getAb<Mat>(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm,
-                         cv::Affine3f pose, int level, cv::Matx66f& A, cv::Vec6f& b) const
-{
-    CV_TRACE_FUNCTION();
-
-    CV_Assert(oldPts.size() == oldNrm.size());
-    CV_Assert(newPts.size() == newNrm.size());
-
-    ABtype sumAB = ABtype::zeros();
-    Mutex mutex;
-    const Points  op(oldPts), on(oldNrm);
-    const Normals np(newPts), nn(newNrm);
-    GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose,
-                         intrinsics.scale(level).makeProjector(),
-                         distanceThreshold*distanceThreshold, std::cos(angleThreshold));
-    Range range(0, newPts.rows);
-    const int nstripes = -1;
-    parallel_for_(range, invoker, nstripes);
-
-    // splitting AB matrix to A and b
-    for(int i = 0; i < 6; i++)
-    {
-        // augment lower triangle of A by symmetry
-        for(int j = i; j < 6; j++)
-        {
-            A(i, j) = A(j, i) = sumAB(i, j);
-        }
-
-        b(i) = sumAB(i, 6);
-    }
-}
-
-///////// GPU implementation /////////
-
-#ifdef HAVE_OPENCL
-
-template <>
-void ICPImpl::getAb<UMat>(const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm,
-                          Affine3f pose, int level, Matx66f &A, Vec6f &b) const
-{
-    CV_TRACE_FUNCTION();
-
-    Size oldSize = oldPts.size(), newSize = newPts.size();
-    CV_Assert(oldSize == oldNrm.size());
-    CV_Assert(newSize == newNrm.size());
-
-    // calculate 1x7 vector ab to produce b and upper triangle of A:
-    // [A|b] = ab*(ab^t)
-    // and then reduce it across work groups
-
-    cv::String errorStr;
-    ocl::ProgramSource source = ocl::_3d::icp_oclsrc;
-    cv::String options = "-cl-mad-enable";
-    ocl::Kernel k;
-    k.create("getAb", source, options, &errorStr);
-
-    if(k.empty())
-        throw std::runtime_error("Failed to create kernel: " + errorStr);
-
-    size_t globalSize[2];
-    globalSize[0] = (size_t)newPts.cols;
-    globalSize[1] = (size_t)newPts.rows;
-
-    const ocl::Device& device = ocl::Device::getDefault();
-    size_t wgsLimit = device.maxWorkGroupSize();
-    size_t memSize = device.localMemSize();
-    // local memory should keep upperTriangles for all threads in group for reduce
-    const size_t ltsz = UTSIZE*sizeof(float);
-    const size_t lcols = 32;
-    size_t lrows = min(memSize/ltsz, wgsLimit)/lcols;
-    // round lrows down to 2^n
-    lrows = roundDownPow2(lrows);
-    size_t localSize[2] = {lcols, lrows};
-    Size ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]),
-                 (int)divUp(globalSize[1], (unsigned int)localSize[1]));
-
-    // size of local buffer for group-wide reduce
-    size_t lsz = localSize[0]*localSize[1]*ltsz;
-
-    Intr::Projector proj = intrinsics.scale(level).makeProjector();
-    Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy);
-
-    UMat& groupedSumGpu = groupedSumBuffers[level];
-    groupedSumGpu.create(Size(ngroups.width*UTSIZE, ngroups.height),
-                         CV_32F);
-    groupedSumGpu.setTo(0);
-
-    // TODO: optimization possible:
-    // samplers instead of oldPts/oldNrm (mask needed)
-    k.args(ocl::KernelArg::ReadOnlyNoSize(oldPts),
-           ocl::KernelArg::ReadOnlyNoSize(oldNrm),
-           oldSize,
-           ocl::KernelArg::ReadOnlyNoSize(newPts),
-           ocl::KernelArg::ReadOnlyNoSize(newNrm),
-           newSize,
-           ocl::KernelArg::Constant(pose.matrix.val,
-                                    sizeof(pose.matrix.val)),
-           fxy.val, cxy.val,
-           distanceThreshold*distanceThreshold,
-           std::cos(angleThreshold),
-           ocl::KernelArg::Local(lsz),
-           ocl::KernelArg::WriteOnlyNoSize(groupedSumGpu)
-           );
-
-    if(!k.run(2, globalSize, localSize, true))
-        throw std::runtime_error("Failed to run kernel");
-
-    float upperTriangle[UTSIZE];
-    for(int i = 0; i < UTSIZE; i++)
-        upperTriangle[i] = 0;
-
-    Mat groupedSumCpu = groupedSumGpu.getMat(ACCESS_READ);
-
-    for(int y = 0; y < ngroups.height; y++)
-    {
-        const float* rowr = groupedSumCpu.ptr<float>(y);
-        for(int x = 0; x < ngroups.width; x++)
-        {
-            const float* p = rowr + x*UTSIZE;
-            for(int j = 0; j < UTSIZE; j++)
-            {
-                upperTriangle[j] += p[j];
-            }
-        }
-    }
-    groupedSumCpu.release();
-
-    ABtype sumAB = ABtype::zeros();
-    int pos = 0;
-    for(int i = 0; i < 6; i++)
-    {
-        for(int j = i; j < 7; j++)
-        {
-            sumAB(i, j) = upperTriangle[pos++];
-        }
-    }
-
-    // splitting AB matrix to A and b
-    for(int i = 0; i < 6; i++)
-    {
-        // augment lower triangle of A by symmetry
-        for(int j = i; j < 6; j++)
-        {
-            A(i, j) = A(j, i) = sumAB(i, j);
-        }
-
-        b(i) = sumAB(i, 6);
-    }
-}
-
-#endif
-
-///
-
-
-cv::Ptr<ICP> makeICP(const cv::Intr _intrinsics, const std::vector<int> &_iterations,
-                     float _angleThreshold, float _distanceThreshold)
-{
-    return makePtr<ICPImpl>(_intrinsics.getMat(), _iterations, _angleThreshold, _distanceThreshold);
-}
-
-} // namespace kinfu
-} // namespace cv
diff --git a/modules/3d/src/rgbd/fast_icp.hpp b/modules/3d/src/rgbd/fast_icp.hpp
deleted file mode 100644
index b82c217b7f..0000000000
--- a/modules/3d/src/rgbd/fast_icp.hpp
+++ /dev/null
@@ -1,42 +0,0 @@
-// 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_FAST_ICP_HPP
-#define OPENCV_3D_FAST_ICP_HPP
-
-#include "../precomp.hpp"
-#include "utils.hpp"
-
-namespace cv {
-namespace kinfu {
-
-class ICP
-{
-public:
-    ICP(const cv::Matx33f _intrinsics, const std::vector<int> &_iterations, float _angleThreshold, float _distanceThreshold);
-
-    virtual bool estimateTransform(cv::Affine3f& transform,
-                                   InputArray oldPoints, InputArray oldNormals,
-                                   InputArray newPoints, InputArray newNormals
-                                   ) const = 0;
-    virtual ~ICP() { }
-
-protected:
-
-    std::vector<int> iterations;
-    float angleThreshold;
-    float distanceThreshold;
-    cv::Intr intrinsics;
-};
-
-cv::Ptr<ICP> makeICP(const cv::Intr _intrinsics, const std::vector<int> &_iterations,
-                     float _angleThreshold, float _distanceThreshold);
-
-} // namespace kinfu
-} // namespace cv
-
-#endif // include guard
diff --git a/modules/3d/src/rgbd/normal.cpp b/modules/3d/src/rgbd/normal.cpp
index 8340935a1a..886c953f59 100644
--- a/modules/3d/src/rgbd/normal.cpp
+++ b/modules/3d/src/rgbd/normal.cpp
@@ -16,6 +16,11 @@ 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
@@ -24,7 +29,7 @@ T inline norm_vec(const Vec<T, 3>& vec)
 template<typename T>
 Mat_<T> computeRadius(const Mat& points)
 {
-    typedef Vec<T, 3> PointT;
+    typedef Vec<T, 4> PointT;
 
     // Compute the
     Size size(points.cols, points.rows);
@@ -54,7 +59,8 @@ void computeThetaPhi(int rows, int cols, const Matx<T, 3, 3>& K, Mat& cos_theta,
     Mat points3d;
     depthTo3d(depth_image, Mat(K), points3d);
 
-    typedef Vec<T, 3> Vec3T;
+    //typedef Vec<T, 3> Vec3T;
+    typedef Vec<T, 4> Vec4T;
 
     cos_theta = Mat_<T>(rows, cols);
     sin_theta = Mat_<T>(rows, cols);
@@ -65,8 +71,8 @@ void computeThetaPhi(int rows, int cols, const Matx<T, 3, 3>& K, Mat& cos_theta,
     {
         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 Vec3T* row_points = points3d.ptr <Vec3T>(y),
-               * row_points_end = points3d.ptr <Vec3T>(y) + points3d.cols;
+        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)
@@ -100,6 +106,20 @@ inline void signNormal(const Vec<T, 3>& normal_in, Vec<T, 3>& normal_out)
     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
@@ -121,6 +141,25 @@ inline void signNormal(T a, T b, T c, Vec<T, 3>& normal)
         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;
+    }
+}
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -227,7 +266,7 @@ public:
         calcRadiusAnd3d(points3d_ori, points3d, radius);
 
         // Get the normals
-        normals_out.create(points3d_ori.size(), CV_MAKETYPE(dtype, 3));
+        normals_out.create(points3d_ori.size(), CV_MAKETYPE(dtype, 4));
         if (points3d_in.empty())
             return;
 
@@ -262,6 +301,7 @@ 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) :
@@ -345,13 +385,15 @@ public:
         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);
+        //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
             {
@@ -366,7 +408,8 @@ public:
 
     virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
     {
-        CV_Assert(points3d_ori.channels() == 3);
+        //CV_Assert(points3d_ori.channels() == 3);
+        CV_Assert(points3d_ori.channels() == 4);
         CV_Assert(points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
     }
 
@@ -401,6 +444,7 @@ 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;
 
@@ -421,7 +465,8 @@ public:
     {
         // Only focus on the depth image for LINEMOD
         Mat depth_in;
-        if (points3d.channels() == 3)
+        //if (points3d.channels() == 3)
+        if (points3d.channels() == 4)
         {
             std::vector<Mat> channels;
             split(points3d, channels);
@@ -496,7 +541,7 @@ public:
         for (int y = r; y < this->rows - r - 1; ++y)
         {
             const DepthDepth* p_line = reinterpret_cast<const DepthDepth*>(depthIn.ptr(y, r));
-            Vec3T* normal = normals.ptr<Vec3T>(y, r);
+            Vec4T* normal = normals.ptr<Vec4T>(y, r);
 
             for (int x = r; x < this->cols - r - 1; ++x)
             {
@@ -548,7 +593,7 @@ public:
 
     virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
     {
-        CV_Assert(((points3d_ori.channels() == 3) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)) ||
+        CV_Assert(((points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)) ||
                   ((points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
     }
 
@@ -569,6 +614,7 @@ 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) :
@@ -679,13 +725,13 @@ public:
         sepFilter2D(r, r_phi, r.depth(), kx_dy_, ky_dy_);
 
         // Fill the result matrix
-        Mat_<Vec3T> normals(this->rows, this->cols);
+        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];
-        Vec3T* normal = normals[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))
@@ -693,6 +739,7 @@ public:
                 (*normal)[0] = *r_ptr;
                 (*normal)[1] = *r_ptr;
                 (*normal)[2] = *r_ptr;
+                (*normal)[3] = 0;
             }
             else
             {
@@ -706,15 +753,15 @@ public:
         }
 
         remap(normals, normals_out, invxy_, invfxy_, INTER_LINEAR);
-        normal = normals_out.ptr<Vec3T>(0);
-        Vec3T* normal_end = normal + this->rows * this->cols;
+        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);
     }
 
     virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
     {
-        CV_Assert(((points3d_ori.channels() == 3) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
+        CV_Assert(((points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
     }
 
     // Cached data
diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp
index 37ba96f21e..9705b9a7e0 100644
--- a/modules/3d/src/rgbd/odometry.cpp
+++ b/modules/3d/src/rgbd/odometry.cpp
@@ -2,974 +2,295 @@
 // 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
 
-// FastICPOdometry is based on kinfu-remake code
-// Copyright(c) 2012, Anatoly Baksheev
-// All rights reserved.
-
 #include "../precomp.hpp"
 #include "utils.hpp"
-#include "fast_icp.hpp"
+#include "opencv2/3d/odometry.hpp"
+#include "odometry_functions.hpp"
 
 namespace cv
 {
 
-enum
+class Odometry::Impl
 {
-    RGBD_ODOMETRY = 1,
-    ICP_ODOMETRY = 2,
-    MERGED_ODOMETRY = RGBD_ODOMETRY + ICP_ODOMETRY
+private:
+
+public:
+    Impl() {};
+    virtual ~Impl() {};
+    virtual OdometryFrame createOdometryFrame() const = 0;
+    virtual void prepareFrame(OdometryFrame& frame) = 0;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) = 0;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const = 0;
 };
 
-static const int sobelSize = 3;
-static const double sobelScale = 1./8.;
-static const int normalWinSize = 5;
-static const RgbdNormals::RgbdNormalsMethod normalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
 
-static
-void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector<Matx33f>& pyramidCameraMatrix)
+class OdometryICP : public Odometry::Impl
 {
-    pyramidCameraMatrix.resize(levels);
+private:
+    OdometrySettings settings;
+    OdometryAlgoType algtype;
 
-    for(int i = 0; i < levels; i++)
+public:
+    OdometryICP(OdometrySettings settings, OdometryAlgoType algtype);
+    ~OdometryICP();
+
+    virtual OdometryFrame createOdometryFrame() const override;
+    virtual void prepareFrame(OdometryFrame& frame) override;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
+};
+
+OdometryICP::OdometryICP(OdometrySettings _settings, OdometryAlgoType _algtype)
+{
+    this->settings = _settings;
+    this->algtype = _algtype;
+}
+
+OdometryICP::~OdometryICP()
+{
+}
+
+OdometryFrame OdometryICP::createOdometryFrame() const
+{
+#ifdef HAVE_OPENCL
+    return OdometryFrame(OdometryFrameStoreType::UMAT);
+#else
+    return OdometryFrame(OdometryFrameStoreType::MAT);
+#endif
+}
+
+void OdometryICP::prepareFrame(OdometryFrame& frame)
+{
+    prepareICPFrame(frame, frame, this->settings, this->algtype);
+}
+
+void OdometryICP::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
+{
+    prepareICPFrame(srcFrame, dstFrame, 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;
+    Mat miterCounts;
+    settings.getIterCounts(miterCounts);
+    for (int i = 0; i < miterCounts.size().height; i++)
+        iterCounts.push_back(miterCounts.at<int>(i));
+    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;
+}
+
+
+class OdometryRGB : public Odometry::Impl
+{
+private:
+    OdometrySettings settings;
+    OdometryAlgoType algtype;
+
+public:
+    OdometryRGB(OdometrySettings settings, OdometryAlgoType algtype);
+    ~OdometryRGB();
+
+    virtual OdometryFrame createOdometryFrame() const override;
+    virtual void prepareFrame(OdometryFrame& frame) override;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
+};
+
+OdometryRGB::OdometryRGB(OdometrySettings _settings, OdometryAlgoType _algtype)
+{
+    this->settings = _settings;
+    this->algtype = _algtype;
+}
+
+OdometryRGB::~OdometryRGB()
+{
+}
+
+OdometryFrame OdometryRGB::createOdometryFrame() const
+{
+    return OdometryFrame(OdometryFrameStoreType::MAT);
+}
+
+void OdometryRGB::prepareFrame(OdometryFrame& frame)
+{
+    prepareRGBFrame(frame, frame, this->settings);
+}
+
+void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
+{
+    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;
+    Mat miterCounts;
+    settings.getIterCounts(miterCounts);
+    CV_CheckTypeEQ(miterCounts.type(), CV_32S, "");
+    for (int i = 0; i < miterCounts.size().height; i++)
+        iterCounts.push_back(miterCounts.at<int>(i));
+    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;
+}
+
+
+class OdometryRGBD : public Odometry::Impl
+{
+private:
+    OdometrySettings settings;
+    OdometryAlgoType algtype;
+
+public:
+    OdometryRGBD(OdometrySettings settings, OdometryAlgoType algtype);
+    ~OdometryRGBD();
+
+    virtual OdometryFrame createOdometryFrame() const override;
+    virtual void prepareFrame(OdometryFrame& frame) override;
+    virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override;
+    virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override;
+};
+
+OdometryRGBD::OdometryRGBD(OdometrySettings _settings, OdometryAlgoType _algtype)
+{
+    this->settings = _settings;
+    this->algtype = _algtype;
+}
+
+OdometryRGBD::~OdometryRGBD()
+{
+}
+
+OdometryFrame OdometryRGBD::createOdometryFrame() const
+{
+    return OdometryFrame(OdometryFrameStoreType::MAT);
+}
+
+void OdometryRGBD::prepareFrame(OdometryFrame& frame)
+{
+    prepareRGBDFrame(frame, frame, this->settings, this->algtype);
+}
+
+void OdometryRGBD::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
+{
+    prepareRGBDFrame(srcFrame, dstFrame, 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;
+    Mat miterCounts;
+    settings.getIterCounts(miterCounts);
+    for (int i = 0; i < miterCounts.size().height; i++)
+        iterCounts.push_back(miterCounts.at<int>(i));
+    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;
+}
+
+
+Odometry::Odometry()
+{
+    OdometrySettings settings;
+    this->impl = makePtr<OdometryICP>(settings, OdometryAlgoType::COMMON);
+}
+
+Odometry::Odometry(OdometryType otype, OdometrySettings settings, OdometryAlgoType algtype)
+{
+    switch (otype)
     {
-        Matx33f levelCameraMatrix = (i == 0) ? cameraMatrix : 0.5f * pyramidCameraMatrix[i-1];
-        levelCameraMatrix(2, 2) = 1.0;
-        pyramidCameraMatrix[i] = levelCameraMatrix;
-    }
-}
-
-static inline
-void checkImage(InputArray image)
-{
-    if(image.empty())
-        CV_Error(Error::StsBadSize, "Image is empty.");
-    if(image.type() != CV_8UC1)
-        CV_Error(Error::StsBadSize, "Image type has to be CV_8UC1.");
-}
-
-static inline
-void checkDepth(InputArray depth, const Size& imageSize)
-{
-    if(depth.empty())
-        CV_Error(Error::StsBadSize, "Depth is empty.");
-    if(depth.size() != imageSize)
-        CV_Error(Error::StsBadSize, "Depth has to have the size equal to the image size.");
-    if(depth.type() != CV_32FC1)
-        CV_Error(Error::StsBadSize, "Depth type has to be CV_32FC1.");
-}
-
-static inline
-void checkMask(InputArray mask, const Size& imageSize)
-{
-    if(!mask.empty())
-    {
-        if(mask.size() != imageSize)
-            CV_Error(Error::StsBadSize, "Mask has to have the size equal to the image size.");
-        if(mask.type() != CV_8UC1)
-            CV_Error(Error::StsBadSize, "Mask type has to be CV_8UC1.");
-    }
-}
-
-static inline
-void checkNormals(InputArray normals, const Size& depthSize)
-{
-    if(normals.size() != depthSize)
-        CV_Error(Error::StsBadSize, "Normals has to have the size equal to the depth size.");
-    if(normals.type() != CV_32FC3)
-        CV_Error(Error::StsBadSize, "Normals type has to be CV_32FC3.");
-}
-
-static
-void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage, size_t levelCount)
-{
-    if(!pyramidImage.empty())
-    {
-        size_t nLevels = pyramidImage.size(-1).width;
-        if(nLevels < levelCount)
-            CV_Error(Error::StsBadSize, "Levels count of pyramidImage has to be equal or less than size of iterCounts.");
-
-        CV_Assert(pyramidImage.size(0) == image.size());
-        for(size_t i = 0; i < nLevels; i++)
-            CV_Assert(pyramidImage.type((int)i) == image.type());
-    }
-    else
-        buildPyramid(image, pyramidImage, (int)levelCount - 1);
-}
-
-static
-void preparePyramidDepth(InputArray depth, InputOutputArrayOfArrays pyramidDepth, size_t levelCount)
-{
-    if(!pyramidDepth.empty())
-    {
-        size_t nLevels = pyramidDepth.size(-1).width;
-        if(nLevels < levelCount)
-            CV_Error(Error::StsBadSize, "Levels count of pyramidDepth has to be equal or less than size of iterCounts.");
-
-        CV_Assert(pyramidDepth.size(0) == depth.size());
-        for(size_t i = 0; i < nLevels; i++)
-            CV_Assert(pyramidDepth.type((int)i) == depth.type());
-    }
-    else
-        buildPyramid(depth, pyramidDepth, (int)levelCount - 1);
-}
-
-template<typename TMat>
-static TMat getTMat(InputArray, int = -1);
-
-template<>
-Mat getTMat<Mat>(InputArray a, int i)
-{
-    return a.getMat(i);
-}
-
-template<>
-UMat getTMat<UMat>(InputArray a, int i)
-{
-    return a.getUMat(i);
-}
-
-template<typename TMat>
-static TMat& getTMatRef(InputOutputArray, int = -1);
-
-template<>
-Mat& getTMatRef<Mat>(InputOutputArray a, int i)
-{
-    return a.getMatRef(i);
-}
-
-//TODO: uncomment it when it's in use
-//template<>
-//UMat& getTMatRef<UMat>(InputOutputArray a, int i)
-//{
-//    return a.getUMatRef(i);
-//}
-
-template<typename TMat>
-static
-void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth,
-                        InputArrayOfArrays pyramidNormal,
-                        InputOutputArrayOfArrays pyramidMask)
-{
-    minDepth = std::max(0.f, minDepth);
-
-    int nLevels = pyramidDepth.size(-1).width;
-    if(!pyramidMask.empty())
-    {
-        if(pyramidMask.size(-1).width != nLevels)
-            CV_Error(Error::StsBadSize, "Levels count of pyramidMask has to be equal to size of pyramidDepth.");
-
-        for(int i = 0; i < pyramidMask.size(-1).width; i++)
-        {
-            CV_Assert(pyramidMask.size(i) == pyramidDepth.size(i));
-            CV_Assert(pyramidMask.type(i) == CV_8UC1);
-        }
-    }
-    else
-    {
-        TMat validMask;
-        if(mask.empty())
-            validMask = TMat(pyramidDepth.size(0), CV_8UC1, Scalar(255));
-        else
-            validMask = getTMat<TMat>(mask, -1).clone();
-
-        buildPyramid(validMask, pyramidMask, nLevels - 1);
-
-        for(int i = 0; i < pyramidMask.size(-1).width; i++)
-        {
-            TMat levelDepth = getTMat<TMat>(pyramidDepth, i).clone();
-            patchNaNs(levelDepth, 0);
-
-            TMat& levelMask = getTMatRef<TMat>(pyramidMask, i);
-            TMat gtmin, ltmax, tmpMask;
-            cv::compare(levelDepth, Scalar(minDepth), gtmin, CMP_GT);
-            cv::compare(levelDepth, Scalar(maxDepth), ltmax, CMP_LT);
-            cv::bitwise_and(gtmin, ltmax, tmpMask);
-            cv::bitwise_and(levelMask, tmpMask, levelMask);
-
-            if(!pyramidNormal.empty())
-            {
-                CV_Assert(pyramidNormal.type(i) == CV_32FC3);
-                CV_Assert(pyramidNormal.size(i) == pyramidDepth.size(i));
-                TMat levelNormal = getTMat<TMat>(pyramidNormal, i).clone();
-
-                TMat validNormalMask;
-                // NaN check
-                cv::compare(levelNormal, levelNormal, validNormalMask, CMP_EQ);
-                CV_Assert(validNormalMask.type() == CV_8UC3);
-
-                std::vector<TMat> channelMasks;
-                split(validNormalMask, channelMasks);
-                TMat tmpChMask;
-                cv::bitwise_and(channelMasks[0], channelMasks[1], tmpChMask);
-                cv::bitwise_and(channelMasks[2], tmpChMask, validNormalMask);
-                cv::bitwise_and(levelMask, validNormalMask, levelMask);
-            }
-        }
-    }
-}
-
-template<typename TMat>
-static
-void preparePyramidCloud(InputArrayOfArrays pyramidDepth, const Matx33f& cameraMatrix, InputOutputArrayOfArrays pyramidCloud)
-{
-    size_t depthSize = pyramidDepth.size(-1).width;
-    size_t cloudSize = pyramidCloud.size(-1).width;
-    if(!pyramidCloud.empty())
-    {
-        if(cloudSize != depthSize)
-            CV_Error(Error::StsBadSize, "Incorrect size of pyramidCloud.");
-
-        for(size_t i = 0; i < depthSize; i++)
-        {
-            CV_Assert(pyramidCloud.size((int)i) == pyramidDepth.size((int)i));
-            CV_Assert(pyramidCloud.type((int)i) == CV_32FC3);
-        }
-    }
-    else
-    {
-        std::vector<Matx33f> pyramidCameraMatrix;
-        buildPyramidCameraMatrix(cameraMatrix, (int)depthSize, pyramidCameraMatrix);
-
-        pyramidCloud.create((int)depthSize, 1, CV_32FC3, -1);
-        for(size_t i = 0; i < depthSize; i++)
-        {
-            TMat cloud;
-            depthTo3d(getTMat<TMat>(pyramidDepth, (int)i), pyramidCameraMatrix[i], cloud);
-            getTMatRef<TMat>(pyramidCloud, (int)i) = cloud;
-        }
-    }
-}
-
-template<typename TMat>
-static
-void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel)
-{
-    size_t imgLevels = pyramidImage.size(-1).width;
-    size_t sobelLvls = pyramidSobel.size(-1).width;
-    if(!pyramidSobel.empty())
-    {
-        if(sobelLvls != imgLevels)
-            CV_Error(Error::StsBadSize, "Incorrect size of pyramidSobel.");
-
-        for(size_t i = 0; i < sobelLvls; i++)
-        {
-            CV_Assert(pyramidSobel.size((int)i) == pyramidImage.size((int)i));
-            CV_Assert(pyramidSobel.type((int)i) == CV_16SC1);
-        }
-    }
-    else
-    {
-        pyramidSobel.create((int)imgLevels, 1, CV_16SC1, -1);
-        for(size_t i = 0; i < imgLevels; i++)
-        {
-            Sobel(getTMat<TMat>(pyramidImage, (int)i), getTMatRef<TMat>(pyramidSobel, (int)i), CV_16S, dx, dy, sobelSize);
-        }
-    }
-}
-
-static
-void randomSubsetOfMask(InputOutputArray _mask, float part)
-{
-    const int minPointsCount = 1000; // minimum point count (we can process them fast)
-    const int nonzeros = countNonZero(_mask);
-    const int needCount = std::max(minPointsCount, int(_mask.total() * part));
-    if(needCount < nonzeros)
-    {
-        RNG rng;
-        Mat mask = _mask.getMat();
-        Mat subset(mask.size(), CV_8UC1, Scalar(0));
-
-        int subsetSize = 0;
-        while(subsetSize < needCount)
-        {
-            int y = rng(mask.rows);
-            int x = rng(mask.cols);
-            if(mask.at<uchar>(y,x))
-            {
-                subset.at<uchar>(y,x) = 255;
-                mask.at<uchar>(y,x) = 0;
-                subsetSize++;
-            }
-        }
-        _mask.assign(subset);
-    }
-}
-
-static
-void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy,
-                                InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
-                                InputOutputArrayOfArrays pyramidTexturedMask)
-{
-    size_t didxLevels = pyramid_dI_dx.size(-1).width;
-    size_t texLevels = pyramidTexturedMask.size(-1).width;
-    if(!pyramidTexturedMask.empty())
-    {
-        if(texLevels != didxLevels)
-            CV_Error(Error::StsBadSize, "Incorrect size of pyramidTexturedMask.");
-
-        for(size_t i = 0; i < texLevels; i++)
-        {
-            CV_Assert(pyramidTexturedMask.size((int)i) == pyramid_dI_dx.size((int)i));
-            CV_Assert(pyramidTexturedMask.type((int)i) == CV_8UC1);
-        }
-    }
-    else
-    {
-        CV_Assert(minGradMagnitudes.type() == CV_32F);
-        Mat_<float> mgMags = minGradMagnitudes.getMat();
-
-        const float sobelScale2_inv = 1.f / (float)(sobelScale * sobelScale);
-        pyramidTexturedMask.create((int)didxLevels, 1, CV_8UC1, -1);
-        for(size_t i = 0; i < didxLevels; i++)
-        {
-            const float minScaledGradMagnitude2 = mgMags((int)i) * mgMags((int)i) * sobelScale2_inv;
-            const Mat& dIdx = pyramid_dI_dx.getMat((int)i);
-            const Mat& dIdy = pyramid_dI_dy.getMat((int)i);
-
-            Mat texturedMask(dIdx.size(), CV_8UC1, Scalar(0));
-
-            for(int y = 0; y < dIdx.rows; y++)
-            {
-                const short *dIdx_row = dIdx.ptr<short>(y);
-                const short *dIdy_row = dIdy.ptr<short>(y);
-                uchar *texturedMask_row = texturedMask.ptr<uchar>(y);
-                for(int x = 0; x < dIdx.cols; x++)
-                {
-                    float magnitude2 = static_cast<float>(dIdx_row[x] * dIdx_row[x] + dIdy_row[x] * dIdy_row[x]);
-                    if(magnitude2 >= minScaledGradMagnitude2)
-                        texturedMask_row[x] = 255;
-                }
-            }
-            Mat texMask = texturedMask & pyramidMask.getMat((int)i);
-
-            randomSubsetOfMask(texMask, (float)maxPointsPart);
-            pyramidTexturedMask.getMatRef((int)i) = texMask;
-        }
-    }
-}
-
-static
-void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals)
-{
-    size_t depthLevels = pyramidDepth.size(-1).width;
-    size_t normalsLevels = pyramidNormals.size(-1).width;
-    if(!pyramidNormals.empty())
-    {
-        if(normalsLevels != depthLevels)
-            CV_Error(Error::StsBadSize, "Incorrect size of pyramidNormals.");
-
-        for(size_t i = 0; i < normalsLevels; i++)
-        {
-            CV_Assert(pyramidNormals.size((int)i) == pyramidDepth.size((int)i));
-            CV_Assert(pyramidNormals.type((int)i) == CV_32FC3);
-        }
-    }
-    else
-    {
-        buildPyramid(normals, pyramidNormals, (int)depthLevels - 1);
-        // renormalize normals
-        for(size_t i = 1; i < depthLevels; i++)
-        {
-            Mat& currNormals = pyramidNormals.getMatRef((int)i);
-            for(int y = 0; y < currNormals.rows; y++)
-            {
-                Point3f* normals_row = currNormals.ptr<Point3f>(y);
-                for(int x = 0; x < currNormals.cols; x++)
-                {
-                    double nrm = norm(normals_row[x]);
-                    normals_row[x] *= 1./nrm;
-                }
-            }
-        }
-    }
-}
-
-static
-void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart,
-                               InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask)
-{
-    size_t maskLevels = pyramidMask.size(-1).width;
-    size_t norMaskLevels = pyramidNormalsMask.size(-1).width;
-    if(!pyramidNormalsMask.empty())
-    {
-        if(norMaskLevels != maskLevels)
-            CV_Error(Error::StsBadSize, "Incorrect size of pyramidNormalsMask.");
-
-        for(size_t i = 0; i < norMaskLevels; i++)
-        {
-            CV_Assert(pyramidNormalsMask.size((int)i) == pyramidMask.size((int)i));
-            CV_Assert(pyramidNormalsMask.type((int)i) == pyramidMask.type((int)i));
-        }
-    }
-    else
-    {
-        pyramidNormalsMask.create((int)maskLevels, 1, CV_8U, -1);
-        for(size_t i = 0; i < maskLevels; i++)
-        {
-            Mat& normalsMask = pyramidNormalsMask.getMatRef((int)i);
-            normalsMask = pyramidMask.getMat((int)i).clone();
-
-            const Mat normals = pyramidNormals.getMat((int)i);
-            for(int y = 0; y < normalsMask.rows; y++)
-            {
-                const Vec3f *normals_row = normals.ptr<Vec3f>(y);
-                uchar *normalsMask_row = normalsMask.ptr<uchar>(y);
-                for(int x = 0; x < normalsMask.cols; x++)
-                {
-                    Vec3f n = normals_row[x];
-                    if(cvIsNaN(n[0]))
-                    {
-                        CV_DbgAssert(cvIsNaN(n[1]) && cvIsNaN(n[2]));
-                        normalsMask_row[x] = 0;
-                    }
-                }
-            }
-            randomSubsetOfMask(normalsMask, (float)maxPointsPart);
-        }
-    }
-}
-
-///////////////////////////////////////////////////////////////////////////////////////
-
-static
-void computeProjectiveMatrix(const Mat& ksi, Mat& Rt)
-{
-    CV_Assert(ksi.size() == Size(1,6) && ksi.type() == CV_64FC1);
-
-    const double* ksi_ptr = ksi.ptr<const double>();
-    // 0.5 multiplication is here because (dual) quaternions keep half an angle/twist inside
-    Matx44d matdq = (DualQuatd(0, ksi_ptr[0], ksi_ptr[1], ksi_ptr[2],
-                               0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5])*0.5).exp().toMat(QUAT_ASSUME_UNIT);
-
-    matdq.copyTo(Rt);
-}
-
-static
-void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
-                     const Mat& depth0, const Mat& validMask0,
-                     const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
-                     Mat& _corresps)
-{
-    CV_Assert(Rt.type() == CV_64FC1);
-
-    Mat corresps(depth1.size(), CV_16SC2, Scalar::all(-1));
-
-    Matx33d K(_K), K_inv(_K_inv);
-    Rect r(0, 0, depth1.cols, depth1.rows);
-    Mat Kt = Rt(Rect(3,0,1,3)).clone();
-    Kt = K * Kt;
-    const double * Kt_ptr = Kt.ptr<const double>();
-
-    AutoBuffer<float> buf(3 * (depth1.cols + depth1.rows));
-    float *KRK_inv0_u1 = buf.data();
-    float *KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depth1.cols;
-    float *KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depth1.rows;
-    float *KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depth1.cols;
-    float *KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depth1.rows;
-    float *KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depth1.cols;
-    {
-        Mat R = Rt(Rect(0,0,3,3)).clone();
-
-        Mat KRK_inv = K * R * K_inv;
-        const double * KRK_inv_ptr = KRK_inv.ptr<const double>();
-        for(int u1 = 0; u1 < depth1.cols; u1++)
-        {
-            KRK_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1);
-            KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1);
-            KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * u1);
-        }
-
-        for(int v1 = 0; v1 < depth1.rows; v1++)
-        {
-            KRK_inv1_v1_plus_KRK_inv2[v1] = (float)(KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]);
-            KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]);
-            KRK_inv7_v1_plus_KRK_inv8[v1] = (float)(KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]);
-        }
-    }
-
-    int correspCount = 0;
-    for(int v1 = 0; v1 < depth1.rows; v1++)
-    {
-        const float *depth1_row = depth1.ptr<float>(v1);
-        const uchar *mask1_row = selectMask1.ptr<uchar>(v1);
-        for(int u1 = 0; u1 < depth1.cols; u1++)
-        {
-            float d1 = depth1_row[u1];
-            if(mask1_row[u1])
-            {
-                CV_DbgAssert(!cvIsNaN(d1));
-                float transformed_d1 = static_cast<float>(d1 * (KRK_inv6_u1[u1] + KRK_inv7_v1_plus_KRK_inv8[v1]) +
-                                                          Kt_ptr[2]);
-                if(transformed_d1 > 0)
-                {
-                    float transformed_d1_inv = 1.f / transformed_d1;
-                    int u0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv0_u1[u1] + KRK_inv1_v1_plus_KRK_inv2[v1]) +
-                                                           Kt_ptr[0]));
-                    int v0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv3_u1[u1] + KRK_inv4_v1_plus_KRK_inv5[v1]) +
-                                                           Kt_ptr[1]));
-
-                    if(r.contains(Point(u0,v0)))
-                    {
-                        float d0 = depth0.at<float>(v0,u0);
-                        if(validMask0.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff)
-                        {
-                            CV_DbgAssert(!cvIsNaN(d0));
-                            Vec2s& c = corresps.at<Vec2s>(v0,u0);
-                            if(c[0] != -1)
-                            {
-                                int exist_u1 = c[0], exist_v1 = c[1];
-
-                                float exist_d1 = (float)(depth1.at<float>(exist_v1,exist_u1) *
-                                    (KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + Kt_ptr[2]);
-
-                                if(transformed_d1 > exist_d1)
-                                    continue;
-                            }
-                            else
-                                correspCount++;
-
-                            c = Vec2s((short)u1, (short)v1);
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    _corresps.create(correspCount, 1, CV_32SC4);
-    Vec4i * corresps_ptr = _corresps.ptr<Vec4i>();
-    for(int v0 = 0, i = 0; v0 < corresps.rows; v0++)
-    {
-        const Vec2s* corresps_row = corresps.ptr<Vec2s>(v0);
-        for(int u0 = 0; u0 < corresps.cols; u0++)
-        {
-            const Vec2s& c = corresps_row[u0];
-            if(c[0] != -1)
-                corresps_ptr[i++] = Vec4i(u0,v0,c[0],c[1]);
-        }
-    }
-}
-
-static inline
-void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& 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;
-
-    C[0] = -p3d.z * v1 + p3d.y * v2;
-    C[1] =  p3d.z * v0 - p3d.x * v2;
-    C[2] = -p3d.y * v0 + p3d.x * v1;
-    C[3] = v0;
-    C[4] = v1;
-    C[5] = v2;
-}
-
-static inline
-void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const Point3f& 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;
-    C[0] = -p3d.z * v1 + p3d.y * v2;
-    C[1] =  p3d.z * v0 - p3d.x * v2;
-    C[2] = -p3d.y * v0 + p3d.x * v1;
-}
-
-static inline
-void calcRgbdEquationCoeffsTranslation(double* C, double dIdx, double dIdy, const Point3f& 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;
-    C[0] = v0;
-    C[1] = v1;
-    C[2] = v2;
-}
-
-typedef
-void (*CalcRgbdEquationCoeffsPtr)(double*, double, double, const Point3f&, double, double);
-
-static inline
-void calcICPEquationCoeffs(double* C, const Point3f& p0, const Vec3f& n1)
-{
-    C[0] = -p0.z * n1[1] + p0.y * n1[2];
-    C[1] =  p0.z * n1[0] - p0.x * n1[2];
-    C[2] = -p0.y * n1[0] + p0.x * n1[1];
-    C[3] = n1[0];
-    C[4] = n1[1];
-    C[5] = n1[2];
-}
-
-static inline
-void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Vec3f& n1)
-{
-    C[0] = -p0.z * n1[1] + p0.y * n1[2];
-    C[1] =  p0.z * n1[0] - p0.x * n1[2];
-    C[2] = -p0.y * n1[0] + p0.x * n1[1];
-}
-
-static inline
-void calcICPEquationCoeffsTranslation(double* C, const Point3f& /*p0*/, const Vec3f& n1)
-{
-    C[0] = n1[0];
-    C[1] = n1[1];
-    C[2] = n1[2];
-}
-
-typedef
-void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&);
-
-static
-void calcRgbdLsmMatrices(const Mat& image0, const Mat& cloud0, const Mat& Rt,
-               const Mat& image1, const Mat& dI_dx1, const Mat& dI_dy1,
-               const Mat& corresps, double fx, double fy, double sobelScaleIn,
-               Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim)
-{
-    AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
-    AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
-    double* AtB_ptr = AtB.ptr<double>();
-
-    const int correspsCount = corresps.rows;
-
-    CV_Assert(Rt.type() == CV_64FC1);
-    const double * Rt_ptr = Rt.ptr<const double>();
-
-    AutoBuffer<float> diffs(correspsCount);
-    float* diffs_ptr = diffs.data();
-
-    const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
-
-    double sigma = 0;
-    for(int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
-    {
-         const Vec4i& c = corresps_ptr[correspIndex];
-         int u0 = c[0], v0 = c[1];
-         int u1 = c[2], v1 = c[3];
-
-         diffs_ptr[correspIndex] = static_cast<float>(static_cast<int>(image0.at<uchar>(v0,u0)) -
-                                                      static_cast<int>(image1.at<uchar>(v1,u1)));
-         sigma += diffs_ptr[correspIndex] * diffs_ptr[correspIndex];
-    }
-    sigma = std::sqrt(sigma/correspsCount);
-
-    std::vector<double> A_buf(transformDim);
-    double* A_ptr = &A_buf[0];
-
-    for(int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
-    {
-         const Vec4i& c = corresps_ptr[correspIndex];
-         int u0 = c[0], v0 = c[1];
-         int u1 = c[2], v1 = c[3];
-
-         double w = sigma + std::abs(diffs_ptr[correspIndex]);
-         w = w > DBL_EPSILON ? 1./w : 1.;
-
-         double w_sobelScale = w * sobelScaleIn;
-
-         const Point3f& p0 = cloud0.at<Point3f>(v0,u0);
-         Point3f tp0;
-         tp0.x = (float)(p0.x * Rt_ptr[0] + p0.y * Rt_ptr[1] + p0.z * Rt_ptr[2] + Rt_ptr[3]);
-         tp0.y = (float)(p0.x * Rt_ptr[4] + p0.y * Rt_ptr[5] + p0.z * Rt_ptr[6] + Rt_ptr[7]);
-         tp0.z = (float)(p0.x * Rt_ptr[8] + p0.y * Rt_ptr[9] + p0.z * Rt_ptr[10] + Rt_ptr[11]);
-
-         func(A_ptr,
-              w_sobelScale * dI_dx1.at<short int>(v1,u1),
-              w_sobelScale * dI_dy1.at<short int>(v1,u1),
-              tp0, fx, fy);
-
-        for(int y = 0; y < transformDim; y++)
-        {
-            double* AtA_ptr = AtA.ptr<double>(y);
-            for(int x = y; x < transformDim; x++)
-                AtA_ptr[x] += A_ptr[y] * A_ptr[x];
-
-            AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
-        }
-    }
-
-    for(int y = 0; y < transformDim; y++)
-        for(int x = y+1; x < transformDim; x++)
-            AtA.at<double>(x,y) = AtA.at<double>(y,x);
-}
-
-static
-void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
-                        const Mat& cloud1, const Mat& normals1,
-                        const Mat& corresps,
-                        Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim)
-{
-    AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
-    AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
-    double* AtB_ptr = AtB.ptr<double>();
-
-    const int correspsCount = corresps.rows;
-
-    CV_Assert(Rt.type() == CV_64FC1);
-    const double * Rt_ptr = Rt.ptr<const double>();
-
-    AutoBuffer<float> diffs(correspsCount);
-    float * diffs_ptr = diffs.data();
-
-    AutoBuffer<Point3f> transformedPoints0(correspsCount);
-    Point3f * tps0_ptr = transformedPoints0.data();
-
-    const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
-
-    double sigma = 0;
-    for(int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
-    {
-        const Vec4i& c = corresps_ptr[correspIndex];
-        int u0 = c[0], v0 = c[1];
-        int u1 = c[2], v1 = c[3];
-
-        const Point3f& p0 = cloud0.at<Point3f>(v0,u0);
-        Point3f tp0;
-        tp0.x = (float)(p0.x * Rt_ptr[0] + p0.y * Rt_ptr[1] + p0.z * Rt_ptr[2] + Rt_ptr[3]);
-        tp0.y = (float)(p0.x * Rt_ptr[4] + p0.y * Rt_ptr[5] + p0.z * Rt_ptr[6] + Rt_ptr[7]);
-        tp0.z = (float)(p0.x * Rt_ptr[8] + p0.y * Rt_ptr[9] + p0.z * Rt_ptr[10] + Rt_ptr[11]);
-
-        Vec3f n1 = normals1.at<Vec3f>(v1, u1);
-        Point3f v = cloud1.at<Point3f>(v1,u1) - tp0;
-
-        tps0_ptr[correspIndex] = tp0;
-        diffs_ptr[correspIndex] = n1[0] * v.x + n1[1] * v.y + n1[2] * v.z;
-        sigma += diffs_ptr[correspIndex] * diffs_ptr[correspIndex];
-    }
-
-    sigma = std::sqrt(sigma/correspsCount);
-
-    std::vector<double> A_buf(transformDim);
-    double* A_ptr = &A_buf[0];
-    for(int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
-    {
-        const Vec4i& c = corresps_ptr[correspIndex];
-        int u1 = c[2], v1 = c[3];
-
-        double w = sigma + std::abs(diffs_ptr[correspIndex]);
-        w = w > DBL_EPSILON ? 1./w : 1.;
-
-        func(A_ptr, tps0_ptr[correspIndex], normals1.at<Vec3f>(v1, u1) * w);
-
-        for(int y = 0; y < transformDim; y++)
-        {
-            double* AtA_ptr = AtA.ptr<double>(y);
-            for(int x = y; x < transformDim; x++)
-                AtA_ptr[x] += A_ptr[y] * A_ptr[x];
-
-            AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
-        }
-    }
-
-    for(int y = 0; y < transformDim; y++)
-        for(int x = y+1; x < transformDim; x++)
-            AtA.at<double>(x,y) = AtA.at<double>(y,x);
-}
-
-static
-bool solveSystem(const Mat& AtA, const Mat& AtB, double detThreshold, Mat& x)
-{
-    double det = determinant(AtA);
-
-    if(fabs (det) < detThreshold || cvIsNaN(det) || cvIsInf(det))
-        return false;
-
-    solve(AtA, AtB, x, DECOMP_CHOLESKY);
-
-    return true;
-}
-
-static
-bool testDeltaTransformation(const Mat& deltaRt, double maxTranslation, double maxRotation)
-{
-    double translation = norm(deltaRt(Rect(3, 0, 1, 3)));
-
-    Mat rvec;
-    Rodrigues(deltaRt(Rect(0,0,3,3)), rvec);
-
-    double rotation = norm(rvec) * 180. / CV_PI;
-
-    return translation <= maxTranslation && rotation <= maxRotation;
-}
-
-static
-bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt,
-                         const Ptr<OdometryFrame>& srcFrame,
-                         const Ptr<OdometryFrame>& dstFrame,
-                         const Matx33f& cameraMatrix,
-                         float maxDepthDiff, const std::vector<int>& iterCounts,
-                         double maxTranslation, double maxRotation,
-                         int method, int transfromType)
-{
-    int transformDim = -1;
-    CalcRgbdEquationCoeffsPtr rgbdEquationFuncPtr = 0;
-    CalcICPEquationCoeffsPtr icpEquationFuncPtr = 0;
-    switch(transfromType)
-    {
-    case Odometry::RIGID_BODY_MOTION:
-        transformDim = 6;
-        rgbdEquationFuncPtr = calcRgbdEquationCoeffs;
-        icpEquationFuncPtr = calcICPEquationCoeffs;
+    case OdometryType::DEPTH:
+        this->impl = makePtr<OdometryICP>(settings, algtype);
         break;
-    case Odometry::ROTATION:
-        transformDim = 3;
-        rgbdEquationFuncPtr = calcRgbdEquationCoeffsRotation;
-        icpEquationFuncPtr = calcICPEquationCoeffsRotation;
+    case OdometryType::RGB:
+        this->impl = makePtr<OdometryRGB>(settings, algtype);
         break;
-    case Odometry::TRANSLATION:
-        transformDim = 3;
-        rgbdEquationFuncPtr = calcRgbdEquationCoeffsTranslation;
-        icpEquationFuncPtr = calcICPEquationCoeffsTranslation;
+    case OdometryType::RGB_DEPTH:
+        this->impl = makePtr<OdometryRGBD>(settings, algtype);
         break;
     default:
-        CV_Error(Error::StsBadArg, "Incorrect transformation type");
+        CV_Error(Error::StsInternal,
+            "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }");
+        break;
     }
 
-    const int minOverdetermScale = 20;
-    const int minCorrespsCount = minOverdetermScale * transformDim;
-
-    std::vector<Matx33f> pyramidCameraMatrix;
-    buildPyramidCameraMatrix(cameraMatrix, (int)iterCounts.size(), pyramidCameraMatrix);
-
-    Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
-    Mat currRt, ksi;
-
-    bool isOk = false;
-    for(int level = (int)iterCounts.size() - 1; level >= 0; level--)
-    {
-        const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level];
-        const Matx33f& levelCameraMatrix_inv = levelCameraMatrix.inv(DECOMP_SVD);
-        const Mat srcLevelDepth, dstLevelDepth;
-        srcFrame->getPyramidAt(srcLevelDepth, OdometryFrame::PYR_DEPTH, level);
-        dstFrame->getPyramidAt(dstLevelDepth, OdometryFrame::PYR_DEPTH, level);
-
-        const double fx = levelCameraMatrix(0, 0);
-        const double fy = levelCameraMatrix(1, 1);
-        const double determinantThreshold = 1e-6;
-
-        Mat AtA_rgbd, AtB_rgbd, AtA_icp, AtB_icp;
-        Mat corresps_rgbd, corresps_icp;
-
-        // Run transformation search on current level iteratively.
-        for(int iter = 0; iter < iterCounts[level]; iter ++)
-        {
-            Mat resultRt_inv = resultRt.inv(DECOMP_SVD);
-
-            const Mat pyramidMask;
-            srcFrame->getPyramidAt(pyramidMask, OdometryFrame::PYR_MASK, level);
-
-            if(method & RGBD_ODOMETRY)
-            {
-                const Mat pyramidTexturedMask;
-                dstFrame->getPyramidAt(pyramidTexturedMask, OdometryFrame::PYR_TEXMASK, level);
-                computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
-                                srcLevelDepth, pyramidMask, dstLevelDepth, pyramidTexturedMask,
-                                maxDepthDiff, corresps_rgbd);
-            }
-
-            if(method & ICP_ODOMETRY)
-            {
-                const Mat pyramidNormalsMask;
-                dstFrame->getPyramidAt(pyramidNormalsMask, OdometryFrame::PYR_NORMMASK, level);
-                computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
-                                srcLevelDepth, pyramidMask, dstLevelDepth, pyramidNormalsMask,
-                                maxDepthDiff, corresps_icp);
-            }
-
-            if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount)
-                break;
-
-            const Mat srcPyrCloud;
-            srcFrame->getPyramidAt(srcPyrCloud, OdometryFrame::PYR_CLOUD, level);
-
-
-            Mat AtA(transformDim, transformDim, CV_64FC1, Scalar(0)), AtB(transformDim, 1, CV_64FC1, Scalar(0));
-            if(corresps_rgbd.rows >= minCorrespsCount)
-            {
-                const Mat srcPyrImage, dstPyrImage, dstPyrIdx, dstPyrIdy;
-                srcFrame->getPyramidAt(srcPyrImage, OdometryFrame::PYR_IMAGE, level);
-                dstFrame->getPyramidAt(dstPyrImage, OdometryFrame::PYR_IMAGE, level);
-                dstFrame->getPyramidAt(dstPyrIdx, OdometryFrame::PYR_DIX, level);
-                dstFrame->getPyramidAt(dstPyrIdy, OdometryFrame::PYR_DIY, level);
-                calcRgbdLsmMatrices(srcPyrImage, srcPyrCloud, resultRt, dstPyrImage, dstPyrIdx, dstPyrIdy,
-                                    corresps_rgbd, fx, fy, sobelScale,
-                                    AtA_rgbd, AtB_rgbd, rgbdEquationFuncPtr, transformDim);
-
-                AtA += AtA_rgbd;
-                AtB += AtB_rgbd;
-            }
-            if(corresps_icp.rows >= minCorrespsCount)
-            {
-                const Mat dstPyrCloud, dstPyrNormals;
-                dstFrame->getPyramidAt(dstPyrCloud, OdometryFrame::PYR_CLOUD, level);
-                dstFrame->getPyramidAt(dstPyrNormals, OdometryFrame::PYR_NORM, level);
-                calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals,
-                                   corresps_icp, AtA_icp, AtB_icp, icpEquationFuncPtr, transformDim);
-                AtA += AtA_icp;
-                AtB += AtB_icp;
-            }
-
-            bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi);
-            if(!solutionExist)
-                break;
-
-            if(transfromType == Odometry::ROTATION)
-            {
-                Mat tmp(6, 1, CV_64FC1, Scalar(0));
-                ksi.copyTo(tmp.rowRange(0,3));
-                ksi = tmp;
-            }
-            else if(transfromType == Odometry::TRANSLATION)
-            {
-                Mat tmp(6, 1, CV_64FC1, Scalar(0));
-                ksi.copyTo(tmp.rowRange(3,6));
-                ksi = tmp;
-            }
-
-            computeProjectiveMatrix(ksi, currRt);
-            resultRt = currRt * resultRt;
-            isOk = true;
-        }
-    }
-    _Rt.create(resultRt.size(), resultRt.type());
-    Mat Rt = _Rt.getMat();
-    resultRt.copyTo(Rt);
-
-    if(isOk)
-    {
-        Mat deltaRt;
-        if(initRt.empty())
-            deltaRt = resultRt;
-        else
-            deltaRt = resultRt * initRt.inv(DECOMP_SVD);
-
-        isOk = testDeltaTransformation(deltaRt, maxTranslation, maxRotation);
-    }
-
-    return isOk;
 }
 
-//
+Odometry::~Odometry()
+{
+}
+
+OdometryFrame Odometry::createOdometryFrame() const
+{
+    return this->impl->createOdometryFrame();
+}
+
+OdometryFrame Odometry::createOdometryFrame(OdometryFrameStoreType matType) const
+{
+    return OdometryFrame(matType);
+}
+
+void Odometry::prepareFrame(OdometryFrame& frame)
+{
+    this->impl->prepareFrame(frame);
+}
+
+void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame)
+{
+    this->impl->prepareFrames(srcFrame, dstFrame);
+}
+
+bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt)
+{
+    //this->prepareFrames(srcFrame, dstFrame);
+    return this->impl->compute(srcFrame, dstFrame, Rt);
+}
+
 
 template<class ImageElemType>
 static void
 warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
-              const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
-              OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
+    const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
+    OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
 {
     CV_Assert(_image.size() == depth.size());
 
     Mat cloud;
     depthTo3d(depth, cameraMatrix, cloud);
 
+    Mat cloud3;
+    cloud3.create(cloud.size(), CV_32FC3);
+    for (int y = 0; y < cloud3.rows; y++)
+    {
+        for (int x = 0; x < cloud3.cols; x++)
+        {
+            Vec4f p = cloud.at<Vec4f>(y, x);
+            cloud3.at<Vec3f>(y, x) = Vec3f(p[0], p[1], p[2]);
+        }
+    }
+
     std::vector<Point2f> points2d;
     Mat transformedCloud;
-    perspectiveTransform(cloud, transformedCloud, Rt);
+    perspectiveTransform(cloud3, transformedCloud, Rt);
     projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
-                  distCoeff, points2d);
+        distCoeff, points2d);
 
     Mat image = _image.getMat();
     Size sz = _image.size();
@@ -984,14 +305,14 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
     {
         //const Point3f* cloud_row = cloud.ptr<Point3f>(y);
         const Point3f* transformedCloud_row = transformedCloud.ptr<Point3f>(y);
-        const Point2f* points2d_row = &points2d[y*sz.width];
+        const Point2f* points2d_row = &points2d[y * sz.width];
         const ImageElemType* image_row = image.ptr<ImageElemType>(y);
         const uchar* mask_row = mask.empty() ? 0 : mask.ptr<uchar>(y);
         for (int x = 0; x < sz.width; x++)
         {
             const float transformed_z = transformedCloud_row[x].z;
             const Point2i p2d = points2d_row[x];
-            if((!mask_row || mask_row[x]) && transformed_z > 0 && rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at<float>(p2d) > transformed_z)
+            if ((!mask_row || mask_row[x]) && transformed_z > 0 && rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at<float>(p2d) > transformed_z)
             {
                 warpedImage.at<ImageElemType>(p2d) = image_row[x];
                 zBuffer.at<float>(p2d) = transformed_z;
@@ -999,10 +320,10 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
         }
     }
 
-    if(warpedMask.needed())
+    if (warpedMask.needed())
         Mat(zBuffer != std::numeric_limits<float>::max()).copyTo(warpedMask);
 
-   if(warpedDepth.needed())
+    if (warpedDepth.needed())
     {
         zBuffer.setTo(std::numeric_limits<float>::quiet_NaN(), zBuffer == std::numeric_limits<float>::max());
         zBuffer.copyTo(warpedDepth);
@@ -1010,8 +331,8 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
 }
 
 void warpFrame(InputArray image, InputArray depth, InputArray mask,
-               InputArray Rt, InputArray cameraMatrix, InputArray distCoeff,
-               OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
+    InputArray Rt, InputArray cameraMatrix, InputArray distCoeff,
+    OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
 {
     if (image.type() == CV_8UC1)
         warpFrameImpl<uchar>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask);
@@ -1021,1456 +342,4 @@ void warpFrame(InputArray image, InputArray depth, InputArray mask,
         CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3");
 }
 
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-template<typename TMat>
-struct OdometryFrameImpl : public OdometryFrame
-{
-    OdometryFrameImpl() : OdometryFrame(), image(), depth(), mask(), normals(), pyramids(N_PYRAMIDS) { }
-    OdometryFrameImpl(InputArray _image, InputArray _depth, InputArray _mask = noArray(), InputArray _normals = noArray(), int _ID = -1);
-    virtual ~OdometryFrameImpl() { }
-
-    virtual void setImage(InputArray  _image) CV_OVERRIDE
-    {
-        image = getTMat<TMat>(_image);
-    }
-    virtual void getImage(OutputArray _image) CV_OVERRIDE
-    {
-        _image.assign(image);
-    }
-    virtual void setDepth(InputArray  _depth) CV_OVERRIDE
-    {
-        depth = getTMat<TMat>(_depth);
-    }
-    virtual void getDepth(OutputArray _depth) CV_OVERRIDE
-    {
-        _depth.assign(depth);
-    }
-    virtual void setMask(InputArray  _mask) CV_OVERRIDE
-    {
-        mask = getTMat<TMat>(_mask);
-    }
-    virtual void getMask(OutputArray _mask) CV_OVERRIDE
-    {
-        _mask.assign(mask);
-    }
-    virtual void setNormals(InputArray  _normals) CV_OVERRIDE
-    {
-        normals = getTMat<TMat>(_normals);
-    }
-    virtual void getNormals(OutputArray _normals) CV_OVERRIDE
-    {
-        _normals.assign(normals);
-    }
-
-    virtual void setPyramidLevels(size_t _nLevels) CV_OVERRIDE
-    {
-        for (auto& p : pyramids)
-        {
-            p.resize(_nLevels, TMat());
-        }
-    }
-
-    virtual size_t getPyramidLevels(OdometryFramePyramidType what = PYR_IMAGE) CV_OVERRIDE
-    {
-        if (what < N_PYRAMIDS)
-            return pyramids[what].size();
-        else
-            return 0;
-    }
-
-    virtual void setPyramidAt(InputArray  _pyrImage, OdometryFramePyramidType pyrType, size_t level) CV_OVERRIDE
-    {
-        TMat img = getTMat<TMat>(_pyrImage);
-        pyramids[pyrType][level] = img;
-    }
-
-    virtual void getPyramidAt(OutputArray _pyrImage, OdometryFramePyramidType pyrType, size_t level) CV_OVERRIDE
-    {
-        TMat img;
-        img = pyramids[pyrType][level];
-        _pyrImage.assign(img);
-    }
-
-    TMat image;
-    TMat depth;
-    TMat mask;
-    TMat normals;
-
-    std::vector< std::vector<TMat> > pyramids;
-};
-
-template<>
-OdometryFrameImpl<Mat>::OdometryFrameImpl(InputArray _image, InputArray _depth, InputArray _mask, InputArray _normals, int _ID) :
-    OdometryFrame(),
-    image(_image.getMat()), depth(_depth.getMat()), mask(_mask.getMat()), normals(_normals.getMat()),
-    pyramids(N_PYRAMIDS)
-{
-    ID = _ID;
 }
-
-template<>
-OdometryFrameImpl<UMat>::OdometryFrameImpl(InputArray _image, InputArray _depth, InputArray _mask, InputArray _normals, int _ID) :
-    OdometryFrame(),
-    image(_image.getUMat()), depth(_depth.getUMat()), mask(_mask.getUMat()), normals(_normals.getUMat()),
-    pyramids(N_PYRAMIDS)
-{
-    ID = _ID;
-}
-
-
-Ptr<OdometryFrame> OdometryFrame::create(InputArray _image, InputArray _depth, InputArray _mask, InputArray _normals, int _ID)
-{
-    bool allEmpty = _image.empty() && _depth.empty() && _mask.empty() && _normals.empty();
-    bool useOcl = (_image.isUMat() || _image.empty()) &&
-                  (_depth.isUMat() || _depth.empty()) &&
-                  (_mask.isUMat()  || _mask.empty()) &&
-                  (_normals.isUMat() || _normals.empty());
-    if (useOcl && !allEmpty)
-        return makePtr<OdometryFrameImpl<UMat>>(_image, _depth, _mask, _normals, _ID);
-    else
-        return makePtr<OdometryFrameImpl<Mat>> (_image, _depth, _mask, _normals, _ID);
-}
-
-
-class OdometryImpl : public Odometry
-{
-public:
-    // initialized outside of a class
-    static const float defaultMaxTranslation;
-    static const float defaultMaxRotation;
-    static const float defaultMinGradientMagnitude;
-    static const std::vector<int> defaultIterCounts;
-    static const cv::Matx33f defaultCameraMatrix;
-    static const float defaultMinDepth;
-    static const float defaultMaxDepth;
-    static const float defaultMaxDepthDiff;
-    static const float defaultMaxPointsPart;
-
-    OdometryImpl(InputArray _cameraMatrix = noArray(),
-                 InputArray _iterCounts = noArray(),
-                 InputArray _minGradientMagnitudes = noArray(),
-                 Odometry::OdometryTransformType _transformType = Odometry::RIGID_BODY_MOTION)
-    {
-        setTransformType(_transformType);
-        setMaxTranslation(defaultMaxTranslation);
-        setMaxRotation(defaultMaxRotation);
-        setCameraMatrix(_cameraMatrix);
-        setIterationCounts(_iterCounts);
-        setMinGradientMagnitudes(_minGradientMagnitudes);
-    }
-
-    virtual ~OdometryImpl() { }
-
-    virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
-                         InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const CV_OVERRIDE
-    {
-        Ptr<OdometryFrame> srcFrame = makeOdometryFrame(srcImage, srcDepth, srcMask);
-        Ptr<OdometryFrame> dstFrame = makeOdometryFrame(dstImage, dstDepth, dstMask);
-
-        return compute(srcFrame, dstFrame, Rt, initRt);
-    }
-
-    virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
-                         OutputArray Rt, InputArray initRt) const CV_OVERRIDE
-    {
-        Size srcSize = prepareFrameCache(srcFrame, OdometryFrame::CACHE_SRC);
-        Size dstSize = prepareFrameCache(dstFrame, OdometryFrame::CACHE_DST);
-
-        if (srcSize != dstSize)
-            CV_Error(Error::StsBadSize, "srcFrame and dstFrame have to have the same size (resolution).");
-
-        return computeImpl(srcFrame, dstFrame, Rt, initRt);
-    }
-
-    virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType /*cacheType*/) const CV_OVERRIDE
-    {
-        if (!frame)
-            CV_Error(Error::StsBadArg, "Null frame pointer.");
-
-        return Size();
-    }
-
-    virtual void getCameraMatrix(OutputArray val) const CV_OVERRIDE
-    {
-        cameraMatrix.copyTo(val);
-    }
-    virtual void setCameraMatrix(InputArray val) CV_OVERRIDE
-    {
-        if (val.empty())
-        {
-            cameraMatrix = defaultCameraMatrix;
-        }
-        else
-        {
-            CV_Assert(val.rows() == 3 && val.cols() == 3 && val.channels() == 1);
-            CV_Assert(val.type() == CV_32F);
-            val.copyTo(cameraMatrix);
-        }
-    }
-
-    virtual Odometry::OdometryTransformType getTransformType() const CV_OVERRIDE
-    {
-        return transformType;
-    }
-    virtual void setTransformType(Odometry::OdometryTransformType val) CV_OVERRIDE
-    {
-        transformType = val;
-    }
-
-    virtual void getIterationCounts(OutputArray val) const CV_OVERRIDE
-    {
-        val.create((int)iterCounts.size(), 1, CV_32S);
-        Mat(iterCounts).copyTo(val);
-    }
-    virtual void setIterationCounts(InputArray val) CV_OVERRIDE
-    {
-        if (val.empty())
-        {
-            iterCounts = defaultIterCounts;
-        }
-        else
-        {
-            CV_Assert(val.type() == CV_32SC1);
-            CV_Assert(val.rows() == 1 || val.cols() == 1);
-            iterCounts.resize(val.rows() * val.cols());
-            val.copyTo(iterCounts);
-
-            minGradientMagnitudes.resize(iterCounts.size(), defaultMinGradientMagnitude);
-        }
-    }
-
-    virtual void getMinGradientMagnitudes(OutputArray val) const CV_OVERRIDE
-    {
-        val.create((int)minGradientMagnitudes.size(), 1, CV_32F);
-        Mat(minGradientMagnitudes).copyTo(val);
-    }
-    virtual void setMinGradientMagnitudes(InputArray val) CV_OVERRIDE
-    {
-        if (val.empty())
-        {
-            minGradientMagnitudes = std::vector<float>(iterCounts.size(), defaultMinGradientMagnitude);
-        }
-        else
-        {
-            CV_Assert(val.type() == CV_32FC1);
-            CV_Assert(val.rows() == 1 || val.cols() == 1);
-            size_t valSize = val.rows() * val.cols();
-            CV_Assert(valSize == iterCounts.size());
-            minGradientMagnitudes.resize(valSize);
-            val.copyTo(minGradientMagnitudes);
-        }
-    }
-
-    /** Get max allowed translation in meters.
-    Found delta transform is considered successful only if the translation is in given limits. */
-    virtual double getMaxTranslation() const CV_OVERRIDE
-    {
-        return maxTranslation;
-    }
-    /** Set max allowed translation in meters.
-    * Found delta transform is considered successful only if the translation is in given limits. */
-    virtual void setMaxTranslation(double val) CV_OVERRIDE
-    {
-        maxTranslation = val;
-    }
-    /** Get max allowed rotation in degrees.
-    * Found delta transform is considered successful only if the rotation is in given limits. */
-    virtual double getMaxRotation() const CV_OVERRIDE
-    {
-        return maxRotation;
-    }
-    /** Set max allowed rotation in degrees.
-    * Found delta transform is considered successful only if the rotation is in given limits. */
-    virtual void setMaxRotation(double val) CV_OVERRIDE
-    {
-        maxRotation = val;
-    }
-
-    virtual bool computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
-                             InputArray initRt) const = 0;
-
-    double maxTranslation, maxRotation;
-
-    Odometry::OdometryTransformType transformType;
-
-    Matx33f cameraMatrix;
-    std::vector<int> iterCounts;
-    std::vector<float> minGradientMagnitudes;
-};
-
-const float OdometryImpl::defaultMaxTranslation = 0.15f;
-const float OdometryImpl::defaultMaxRotation = 15.f;
-const float OdometryImpl::defaultMinGradientMagnitude = 10.f;
-const std::vector<int> OdometryImpl::defaultIterCounts = { 7, 7, 7, 10 };
-const cv::Matx33f OdometryImpl::defaultCameraMatrix = { /* fx, 0, cx*/ 0, 0, 0, /* 0, fy, cy */ 0, 0, 0, /**/ 0, 0, 0 };
-const float OdometryImpl::defaultMinDepth = 0.f;
-const float OdometryImpl::defaultMaxDepth = 4.f;
-const float OdometryImpl::defaultMaxDepthDiff = 0.07f;
-const float OdometryImpl::defaultMaxPointsPart = 0.07f;
-
-//
-
-// Public Odometry classes are pure abstract, therefore a sin of multiple inheritance should be forgiven
-class RgbdOdometryImpl : public OdometryImpl, public RgbdOdometry
-{
-public:
-    /** Constructor.
-     * @param cameraMatrix Camera matrix
-     * @param minDepth Pixels with depth less than minDepth will not be used (in meters)
-     * @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
-     * @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff (in meters)
-     * @param iterCounts Count of iterations on each pyramid level.
-     * @param minGradientMagnitudes For each pyramid level the pixels will be filtered out
-     *                              if they have gradient magnitude less than minGradientMagnitudes[level].
-     * @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart
-     * @param transformType Class of transformation
-     */
-    RgbdOdometryImpl(InputArray _cameraMatrix = noArray(),
-                     float _minDepth = defaultMinDepth,
-                     float _maxDepth = defaultMaxDepth,
-                     float _maxDepthDiff = defaultMaxDepthDiff,
-                     InputArray _iterCounts = noArray(),
-                     InputArray _minGradientMagnitudes = noArray(),
-                     float _maxPointsPart = defaultMaxPointsPart,
-                     Odometry::OdometryTransformType _transformType = Odometry::RIGID_BODY_MOTION) :
-        OdometryImpl(_cameraMatrix, _iterCounts, _minGradientMagnitudes, _transformType)
-    {
-        setMinDepth(_minDepth);
-        setMaxDepth(_maxDepth);
-        setMaxDepthDiff(_maxDepthDiff);
-        setMaxPointsPart(_maxPointsPart);
-    }
-
-    virtual ~RgbdOdometryImpl() { }
-
-    virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const CV_OVERRIDE;
-
-    virtual Ptr<OdometryFrame> makeOdometryFrame(InputArray image, InputArray depth, InputArray mask) const CV_OVERRIDE;
-
-    virtual double getMinDepth() const CV_OVERRIDE
-    {
-        return minDepth;
-    }
-    virtual void setMinDepth(double val) CV_OVERRIDE
-    {
-        minDepth = val;
-    }
-    virtual double getMaxDepth() const CV_OVERRIDE
-    {
-        return maxDepth;
-    }
-    virtual void setMaxDepth(double val) CV_OVERRIDE
-    {
-        maxDepth = val;
-    }
-    virtual double getMaxDepthDiff() const CV_OVERRIDE
-    {
-        return maxDepthDiff;
-    }
-    virtual void setMaxDepthDiff(double val) CV_OVERRIDE
-    {
-        maxDepthDiff = val;
-    }
-    virtual double getMaxPointsPart() const CV_OVERRIDE
-    {
-        return maxPointsPart;
-    }
-    virtual void setMaxPointsPart(double val) CV_OVERRIDE
-    {
-        CV_Assert(val > 0. && val <= 1.);
-        maxPointsPart = val;
-    }
-
-    virtual void getIterationCounts(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getIterationCounts(val);
-    }
-    virtual void setIterationCounts(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setIterationCounts(val);
-    }
-    virtual void getMinGradientMagnitudes(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getMinGradientMagnitudes(val);
-    }
-    virtual void setMinGradientMagnitudes(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setMinGradientMagnitudes(val);
-    }
-    virtual void getCameraMatrix(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getCameraMatrix(val);
-    }
-    virtual void setCameraMatrix(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setCameraMatrix(val);
-    }
-    virtual Odometry::OdometryTransformType getTransformType() const CV_OVERRIDE
-    {
-        return OdometryImpl::getTransformType();
-    }
-    virtual void setTransformType(Odometry::OdometryTransformType val) CV_OVERRIDE
-    {
-        OdometryImpl::setTransformType(val);
-    }
-    virtual double getMaxTranslation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxTranslation();
-    }
-    virtual void setMaxTranslation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxTranslation(val);
-    }
-    virtual double getMaxRotation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxRotation();
-    }
-    virtual void setMaxRotation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxRotation(val);
-    }
-
-    virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
-                         InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcImage, srcDepth, srcMask, dstImage, dstDepth, dstMask, Rt, initRt);
-    }
-
-    virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
-                         OutputArray Rt, InputArray initRt) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcFrame, dstFrame, Rt, initRt);
-    }
-
-protected:
-
-    virtual bool computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
-                             InputArray initRt) const CV_OVERRIDE;
-
-    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
-    /*float*/
-    double minDepth, maxDepth, maxDepthDiff;
-
-    double maxPointsPart;
-};
-
-Ptr<RgbdOdometry> RgbdOdometry::create(InputArray _cameraMatrix, float _minDepth, float _maxDepth,
-                                       float _maxDepthDiff, InputArray _iterCounts,
-                                       InputArray _minGradientMagnitudes, float _maxPointsPart,
-                                       Odometry::OdometryTransformType _transformType)
-{
-    return makePtr<RgbdOdometryImpl>(_cameraMatrix, _minDepth, _maxDepth, _maxDepthDiff, _iterCounts, _minGradientMagnitudes, _maxPointsPart, _transformType);
-}
-
-Ptr<OdometryFrame> RgbdOdometryImpl::makeOdometryFrame(InputArray _image, InputArray _depth, InputArray _mask) const
-{
-    // Can get rid of getMat() calls as soon as this Odometry algorithm supports UMats
-    return OdometryFrame::create(_image.getMat(), _depth.getMat(), _mask.getMat());
-}
-
-
-Size RgbdOdometryImpl::prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const
-{
-    OdometryImpl::prepareFrameCache(frame, cacheType);
-
-    // Can be transformed into template argument in the future
-    // when this algorithm supports OCL UMats too
-    typedef Mat TMat;
-
-    if (frame.dynamicCast<OdometryFrameImpl<UMat>>())
-        CV_Error(cv::Error::Code::StsBadArg, "RgbdOdometry does not support UMats yet");
-
-    TMat image;
-    frame->getImage(image);
-    if(image.empty())
-    {
-        if (frame->getPyramidLevels(OdometryFrame::PYR_IMAGE) > 0)
-        {
-            TMat pyr0;
-            frame->getPyramidAt(pyr0, OdometryFrame::PYR_IMAGE, 0);
-            frame->setImage(pyr0);
-        }
-        else
-            CV_Error(Error::StsBadSize, "Image or pyramidImage have to be set.");
-    }
-    checkImage(image);
-
-    TMat depth;
-    frame->getDepth(depth);
-    if(depth.empty())
-    {
-        if (frame->getPyramidLevels(OdometryFrame::PYR_DEPTH) > 0)
-        {
-            TMat pyr0;
-            frame->getPyramidAt(pyr0, OdometryFrame::PYR_DEPTH, 0);
-            frame->setDepth(pyr0);
-        }
-        else if(frame->getPyramidLevels(OdometryFrame::PYR_CLOUD) > 0)
-        {
-            TMat cloud;
-            frame->getPyramidAt(cloud, OdometryFrame::PYR_CLOUD, 0);
-            std::vector<TMat> xyz;
-            split(cloud, xyz);
-            frame->setDepth(xyz[2]);
-        }
-        else
-            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
-    }
-    checkDepth(depth, image.size());
-
-    TMat mask;
-    frame->getMask(mask);
-    if (mask.empty() && frame->getPyramidLevels(OdometryFrame::PYR_MASK) > 0)
-    {
-        TMat pyr0;
-        frame->getPyramidAt(pyr0, OdometryFrame::PYR_MASK, 0);
-        frame->setMask(pyr0);
-    }
-    checkMask(mask, image.size());
-
-    auto tframe = frame.dynamicCast<OdometryFrameImpl<TMat>>();
-    preparePyramidImage(image, tframe->pyramids[OdometryFrame::PYR_IMAGE], iterCounts.size());
-
-    preparePyramidDepth(depth, tframe->pyramids[OdometryFrame::PYR_DEPTH], iterCounts.size());
-
-    preparePyramidMask<TMat>(mask, tframe->pyramids[OdometryFrame::PYR_DEPTH], (float)minDepth, (float)maxDepth,
-                             tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK]);
-
-    if(cacheType & OdometryFrame::CACHE_SRC)
-        preparePyramidCloud<TMat>(tframe->pyramids[OdometryFrame::PYR_DEPTH], cameraMatrix, tframe->pyramids[OdometryFrame::PYR_CLOUD]);
-
-    if(cacheType & OdometryFrame::CACHE_DST)
-    {
-        preparePyramidSobel<TMat>(tframe->pyramids[OdometryFrame::PYR_IMAGE], 1, 0, tframe->pyramids[OdometryFrame::PYR_DIX]);
-        preparePyramidSobel<TMat>(tframe->pyramids[OdometryFrame::PYR_IMAGE], 0, 1, tframe->pyramids[OdometryFrame::PYR_DIY]);
-        preparePyramidTexturedMask(tframe->pyramids[OdometryFrame::PYR_DIX], tframe->pyramids[OdometryFrame::PYR_DIY], minGradientMagnitudes,
-                                   tframe->pyramids[OdometryFrame::PYR_MASK], maxPointsPart, tframe->pyramids[OdometryFrame::PYR_TEXMASK]);
-    }
-
-    return image.size();
-}
-
-
-bool RgbdOdometryImpl::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt, InputArray initRt) const
-{
-    return RGBDICPOdometryImpl(Rt, initRt.getMat(), srcFrame, dstFrame, cameraMatrix, (float)maxDepthDiff, iterCounts, maxTranslation, maxRotation, RGBD_ODOMETRY, transformType);
-}
-
-//
-
-// Public Odometry classes are pure abstract, therefore a sin of multiple inheritance should be forgiven
-class ICPOdometryImpl : public OdometryImpl, public ICPOdometry
-{
-public:
-
-    /** Constructor.
-     * @param cameraMatrix Camera matrix
-     * @param minDepth Pixels with depth less than minDepth will not be used
-     * @param maxDepth Pixels with depth larger than maxDepth will not be used
-     * @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff
-     * @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart
-     * @param iterCounts Count of iterations on each pyramid level.
-     * @param transformType Class of trasformation
-     */
-    ICPOdometryImpl(InputArray _cameraMatrix = noArray(),
-                    float _minDepth = defaultMinDepth,
-                    float _maxDepth = defaultMaxDepth,
-                    float _maxDepthDiff = defaultMaxDepthDiff,
-                    float _maxPointsPart = defaultMaxPointsPart,
-                    InputArray _iterCounts = noArray(),
-                    Odometry::OdometryTransformType _transformType = Odometry::RIGID_BODY_MOTION) :
-        OdometryImpl(_cameraMatrix, _iterCounts, noArray(), _transformType)
-    {
-        setMinDepth(_minDepth);
-        setMaxDepth(_maxDepth);
-        setMaxDepthDiff(_maxDepthDiff);
-        setMaxPointsPart(_maxPointsPart);
-    }
-
-    virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const CV_OVERRIDE;
-
-    virtual Ptr<OdometryFrame> makeOdometryFrame(InputArray image, InputArray depth, InputArray mask) const CV_OVERRIDE;
-
-    virtual double getMinDepth() const CV_OVERRIDE
-    {
-        return minDepth;
-    }
-    virtual void setMinDepth(double val) CV_OVERRIDE
-    {
-        minDepth = val;
-    }
-    virtual double getMaxDepth() const CV_OVERRIDE
-    {
-        return maxDepth;
-    }
-    virtual void setMaxDepth(double val) CV_OVERRIDE
-    {
-        maxDepth = val;
-    }
-    virtual double getMaxDepthDiff() const CV_OVERRIDE
-    {
-        return maxDepthDiff;
-    }
-    virtual void setMaxDepthDiff(double val) CV_OVERRIDE
-    {
-        maxDepthDiff = val;
-    }
-    virtual double getMaxPointsPart() const CV_OVERRIDE
-    {
-        return maxPointsPart;
-    }
-    virtual void setMaxPointsPart(double val) CV_OVERRIDE
-    {
-        CV_Assert(val > 0. && val <= 1.);
-        maxPointsPart = val;
-    }
-
-    virtual void getIterationCounts(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getIterationCounts(val);
-    }
-    virtual void setIterationCounts(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setIterationCounts(val);
-    }
-    virtual void getMinGradientMagnitudes(OutputArray /*val*/) const CV_OVERRIDE
-    {
-        CV_Error(Error::StsNotImplemented, "This Odometry class does not use minGradientMagnitudes");
-    }
-    virtual void setMinGradientMagnitudes(InputArray /*val*/) CV_OVERRIDE
-    {
-        CV_Error(Error::StsNotImplemented, "This Odometry class does not use minGradientMagnitudes");
-    }
-    virtual void getCameraMatrix(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getCameraMatrix(val);
-    }
-    virtual void setCameraMatrix(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setCameraMatrix(val);
-    }
-    virtual Odometry::OdometryTransformType getTransformType() const CV_OVERRIDE
-    {
-        return OdometryImpl::getTransformType();
-    }
-    virtual void setTransformType(Odometry::OdometryTransformType val) CV_OVERRIDE
-    {
-        OdometryImpl::setTransformType(val);
-    }
-    virtual double getMaxTranslation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxTranslation();
-    }
-    virtual void setMaxTranslation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxTranslation(val);
-    }
-    virtual double getMaxRotation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxRotation();
-    }
-    virtual void setMaxRotation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxRotation(val);
-    }
-
-    virtual Ptr<RgbdNormals> getNormalsComputer() const CV_OVERRIDE
-    {
-        return normalsComputer;
-    }
-
-    virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
-                         InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcImage, srcDepth, srcMask, dstImage, dstDepth, dstMask, Rt, initRt);
-    }
-
-    virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
-                         OutputArray Rt, InputArray initRt) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcFrame, dstFrame, Rt, initRt);
-    }
-
-protected:
-
-    virtual bool computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
-                             InputArray initRt) const CV_OVERRIDE;
-
-    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
-    /*float*/
-    double minDepth, maxDepth, maxDepthDiff;
-    /*float*/
-    double maxPointsPart;
-
-    mutable Ptr<RgbdNormals> normalsComputer;
-};
-
-Ptr<ICPOdometry> ICPOdometry::create(InputArray _cameraMatrix, float _minDepth, float _maxDepth,
-                                     float _maxDepthDiff, float _maxPointsPart, InputArray _iterCounts,
-                                     Odometry::OdometryTransformType _transformType)
-{
-    return makePtr<ICPOdometryImpl>(_cameraMatrix, _minDepth, _maxDepth, _maxDepthDiff, _maxPointsPart, _iterCounts, _transformType);
-}
-
-Ptr<OdometryFrame> ICPOdometryImpl::makeOdometryFrame(InputArray _image, InputArray _depth, InputArray _mask) const
-{
-    // Can get rid of getMat() calls as soon as this Odometry algorithm supports UMats
-    return OdometryFrame::create(_image.getMat(), _depth.getMat(), _mask.getMat());
-}
-
-
-Size ICPOdometryImpl::prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const
-{
-    OdometryImpl::prepareFrameCache(frame, cacheType);
-
-    // Can be transformed into template argument in the future
-    // when this algorithm supports OCL UMats too
-    typedef Mat TMat;
-
-    if (frame.dynamicCast<OdometryFrameImpl<UMat>>())
-        CV_Error(cv::Error::Code::StsBadArg, "ICPOdometry does not support UMats yet");
-
-    TMat depth;
-    frame->getDepth(depth);
-    if(depth.empty())
-    {
-        if (frame->getPyramidLevels(OdometryFrame::PYR_DEPTH))
-        {
-            TMat pyr0;
-            frame->getPyramidAt(pyr0, OdometryFrame::PYR_DEPTH, 0);
-            frame->setDepth(pyr0);
-        }
-        else if(frame->getPyramidLevels(OdometryFrame::PYR_CLOUD))
-        {
-            TMat cloud;
-            frame->getPyramidAt(cloud, OdometryFrame::PYR_CLOUD, 0);
-            std::vector<TMat> xyz;
-            split(cloud, xyz);
-            frame->setDepth(xyz[2]);
-        }
-        else
-            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
-    }
-    checkDepth(depth, depth.size());
-
-    TMat mask;
-    frame->getMask(mask);
-    if (mask.empty() && frame->getPyramidLevels(OdometryFrame::PYR_MASK))
-    {
-        Mat m0;
-        frame->getPyramidAt(m0, OdometryFrame::PYR_MASK, 0);
-        frame->setMask(m0);
-    }
-    checkMask(mask, depth.size());
-
-    auto tframe = frame.dynamicCast<OdometryFrameImpl<TMat>>();
-    preparePyramidDepth(depth, tframe->pyramids[OdometryFrame::PYR_DEPTH], iterCounts.size());
-
-    preparePyramidCloud<TMat>(tframe->pyramids[OdometryFrame::PYR_DEPTH], cameraMatrix, tframe->pyramids[OdometryFrame::PYR_CLOUD]);
-
-    if(cacheType & OdometryFrame::CACHE_DST)
-    {
-        TMat normals;
-        frame->getNormals(normals);
-        if(normals.empty())
-        {
-            if (frame->getPyramidLevels(OdometryFrame::PYR_NORM))
-            {
-                TMat n0;
-                frame->getPyramidAt(n0, OdometryFrame::PYR_NORM, 0);
-                frame->setNormals(n0);
-            }
-            else
-            {
-                Matx33f K;
-                if (!normalsComputer.empty())
-                    normalsComputer->getK(K);
-                if(normalsComputer.empty() ||
-                   normalsComputer->getRows() != depth.rows ||
-                   normalsComputer->getCols() != depth.cols ||
-                   norm(K, cameraMatrix) > FLT_EPSILON)
-                    normalsComputer = RgbdNormals::create(depth.rows,
-                                                          depth.cols,
-                                                          depth.depth(),
-                                                          cameraMatrix,
-                                                          normalWinSize,
-                                                          normalMethod);
-                TMat c0;
-                frame->getPyramidAt(c0, OdometryFrame::PYR_CLOUD, 0);
-                normalsComputer->apply(c0, normals);
-                frame->setNormals(normals);
-            }
-        }
-        checkNormals(normals, depth.size());
-
-        preparePyramidNormals(normals, tframe->pyramids[OdometryFrame::PYR_DEPTH], tframe->pyramids[OdometryFrame::PYR_NORM]);
-
-        preparePyramidMask<TMat>(mask, tframe->pyramids[OdometryFrame::PYR_DEPTH], (float)minDepth, (float)maxDepth,
-                                 tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK]);
-
-        preparePyramidNormalsMask(tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK],
-                                  maxPointsPart, tframe->pyramids[OdometryFrame::PYR_NORMMASK]);
-    }
-    else
-        preparePyramidMask<TMat>(mask, tframe->pyramids[OdometryFrame::PYR_DEPTH], (float)minDepth, (float)maxDepth,
-                                 tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK]);
-
-    return depth.size();
-}
-
-
-bool ICPOdometryImpl::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt, InputArray initRt) const
-{
-    return RGBDICPOdometryImpl(Rt, initRt.getMat(), srcFrame, dstFrame, cameraMatrix, (float)maxDepthDiff, iterCounts, maxTranslation, maxRotation, ICP_ODOMETRY, transformType);
-}
-
-//
-
-// Public Odometry classes are pure abstract, therefore a sin of multiple inheritance should be forgiven
-class RgbdICPOdometryImpl : public OdometryImpl, public RgbdICPOdometry
-{
-public:
-    /** Constructor.
-     * @param cameraMatrix Camera matrix
-     * @param minDepth Pixels with depth less than minDepth will not be used
-     * @param maxDepth Pixels with depth larger than maxDepth will not be used
-     * @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff
-     * @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart
-     * @param iterCounts Count of iterations on each pyramid level.
-     * @param minGradientMagnitudes For each pyramid level the pixels will be filtered out
-     *                              if they have gradient magnitude less than minGradientMagnitudes[level].
-     * @param transformType Class of trasformation
-     */
-    RgbdICPOdometryImpl(InputArray _cameraMatrix = noArray(),
-                        float _minDepth = defaultMinDepth,
-                        float _maxDepth = defaultMaxDepth,
-                        float _maxDepthDiff = defaultMaxDepthDiff,
-                        float _maxPointsPart = defaultMaxPointsPart,
-                        InputArray _iterCounts = noArray(),
-                        InputArray _minGradientMagnitudes = noArray(),
-                        Odometry::OdometryTransformType _transformType = Odometry::RIGID_BODY_MOTION) :
-        OdometryImpl(_cameraMatrix, _iterCounts, _minGradientMagnitudes, _transformType)
-    {
-        setMinDepth(_minDepth);
-        setMaxDepth(_maxDepth);
-        setMaxDepthDiff(_maxDepthDiff);
-        setMaxPointsPart(_maxPointsPart);
-    }
-
-    virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const CV_OVERRIDE;
-
-    virtual Ptr<OdometryFrame> makeOdometryFrame(InputArray image, InputArray depth, InputArray mask) const CV_OVERRIDE;
-
-    virtual double getMinDepth() const CV_OVERRIDE
-    {
-        return minDepth;
-    }
-    virtual void setMinDepth(double val) CV_OVERRIDE
-    {
-        minDepth = val;
-    }
-    virtual double getMaxDepth() const CV_OVERRIDE
-    {
-        return maxDepth;
-    }
-    virtual void setMaxDepth(double val) CV_OVERRIDE
-    {
-        maxDepth = val;
-    }
-    virtual double getMaxDepthDiff() const CV_OVERRIDE
-    {
-        return maxDepthDiff;
-    }
-    virtual void setMaxDepthDiff(double val) CV_OVERRIDE
-    {
-        maxDepthDiff = val;
-    }
-    virtual double getMaxPointsPart() const CV_OVERRIDE
-    {
-        return maxPointsPart;
-    }
-    virtual void setMaxPointsPart(double val) CV_OVERRIDE
-    {
-        CV_Assert(val > 0. && val <= 1.);
-        maxPointsPart = val;
-    }
-
-    virtual void getIterationCounts(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getIterationCounts(val);
-    }
-    virtual void setIterationCounts(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setIterationCounts(val);
-    }
-    virtual void getMinGradientMagnitudes(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getMinGradientMagnitudes(val);
-    }
-    virtual void setMinGradientMagnitudes(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setMinGradientMagnitudes(val);
-    }
-    virtual void getCameraMatrix(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getCameraMatrix(val);
-    }
-    virtual void setCameraMatrix(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setCameraMatrix(val);
-    }
-    virtual Odometry::OdometryTransformType getTransformType() const CV_OVERRIDE
-    {
-        return OdometryImpl::getTransformType();
-    }
-    virtual void setTransformType(Odometry::OdometryTransformType val) CV_OVERRIDE
-    {
-        OdometryImpl::setTransformType(val);
-    }
-    virtual double getMaxTranslation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxTranslation();
-    }
-    virtual void setMaxTranslation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxTranslation(val);
-    }
-    virtual double getMaxRotation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxRotation();
-    }
-    virtual void setMaxRotation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxRotation(val);
-    }
-
-    virtual Ptr<RgbdNormals> getNormalsComputer() const CV_OVERRIDE
-    {
-        return normalsComputer;
-    }
-
-    virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
-                         InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcImage, srcDepth, srcMask, dstImage, dstDepth, dstMask, Rt, initRt);
-    }
-
-    virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
-                         OutputArray Rt, InputArray initRt) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcFrame, dstFrame, Rt, initRt);
-    }
-
-protected:
-
-    virtual bool computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
-                             InputArray initRt) const CV_OVERRIDE;
-
-    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
-    /*float*/
-    double minDepth, maxDepth, maxDepthDiff;
-    /*float*/
-    double maxPointsPart;
-
-    mutable Ptr<RgbdNormals> normalsComputer;
-};
-
-
-Ptr<RgbdICPOdometry> RgbdICPOdometry::create(InputArray _cameraMatrix, float _minDepth, float _maxDepth,
-                                             float _maxDepthDiff, float _maxPointsPart, InputArray _iterCounts,
-                                             InputArray _minGradientMagnitudes,
-                                             Odometry::OdometryTransformType _transformType)
-{
-    return makePtr<RgbdICPOdometryImpl>(_cameraMatrix, _minDepth, _maxDepth, _maxDepthDiff, _maxPointsPart, _iterCounts, _minGradientMagnitudes, _transformType);
-}
-
-Ptr<OdometryFrame> RgbdICPOdometryImpl::makeOdometryFrame(InputArray _image, InputArray _depth, InputArray _mask) const
-{
-    // Can get rid of getMat() calls as soon as this Odometry algorithm supports UMats
-    return OdometryFrame::create(_image.getMat(), _depth.getMat(), _mask.getMat());
-}
-
-
-Size RgbdICPOdometryImpl::prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const
-{
-    OdometryImpl::prepareFrameCache(frame, cacheType);
-
-    // Can be transformed into template argument in the future
-    // when this algorithm supports OCL UMats too
-    typedef Mat TMat;
-
-    if (frame.dynamicCast<OdometryFrameImpl<UMat>>())
-        CV_Error(cv::Error::Code::StsBadArg, "RgbdICPOdometry does not support UMats yet");
-
-    TMat image;
-    frame->getImage(image);
-    if(image.empty())
-    {
-        if (frame->getPyramidLevels(OdometryFrame::PYR_IMAGE))
-        {
-            TMat p0;
-            frame->getPyramidAt(p0, OdometryFrame::PYR_IMAGE, 0);
-            frame->setImage(p0);
-        }
-        else
-            CV_Error(Error::StsBadSize, "Image or pyramidImage have to be set.");
-    }
-    checkImage(image);
-
-    TMat depth;
-    frame->getDepth(depth);
-    if (depth.empty())
-    {
-        if (frame->getPyramidLevels(OdometryFrame::PYR_DEPTH))
-        {
-            TMat d0;
-            frame->getPyramidAt(d0, OdometryFrame::PYR_DEPTH, 0);
-            frame->setDepth(d0);
-        }
-        else if(frame->getPyramidLevels(OdometryFrame::PYR_CLOUD))
-        {
-            TMat cloud;
-            frame->getPyramidAt(cloud, OdometryFrame::PYR_CLOUD, 0);
-            std::vector<TMat> xyz;
-            split(cloud, xyz);
-            frame->setDepth(xyz[2]);
-        }
-        else
-            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
-    }
-    checkDepth(depth, image.size());
-
-    TMat mask;
-    frame->getMask(mask);
-    if(mask.empty() && frame->getPyramidLevels(OdometryFrame::PYR_MASK))
-    {
-        TMat m0;
-        frame->getPyramidAt(m0, OdometryFrame::PYR_MASK, 0);
-        frame->setMask(m0);
-    }
-    checkMask(mask, image.size());
-
-    auto tframe = frame.dynamicCast<OdometryFrameImpl<TMat>>();
-    preparePyramidImage(image, tframe->pyramids[OdometryFrame::PYR_IMAGE], iterCounts.size());
-
-    preparePyramidDepth(depth, tframe->pyramids[OdometryFrame::PYR_DEPTH], iterCounts.size());
-
-    preparePyramidCloud<TMat>(tframe->pyramids[OdometryFrame::PYR_DEPTH], cameraMatrix, tframe->pyramids[OdometryFrame::PYR_CLOUD]);
-
-    if(cacheType & OdometryFrame::CACHE_DST)
-    {
-        TMat normals;
-        frame->getNormals(normals);
-        if (normals.empty())
-        {
-            if (frame->getPyramidLevels(OdometryFrame::PYR_NORM))
-            {
-                TMat n0;
-                frame->getPyramidAt(n0, OdometryFrame::PYR_NORM, 0);
-                frame->setNormals(n0);
-            }
-            else
-            {
-                Matx33f K;
-                if (!normalsComputer.empty())
-                    normalsComputer->getK(K);
-                if(normalsComputer.empty() ||
-                   normalsComputer->getRows() != depth.rows ||
-                   normalsComputer->getCols() != depth.cols ||
-                   norm(K, cameraMatrix) > FLT_EPSILON)
-                    normalsComputer = RgbdNormals::create(depth.rows,
-                                                          depth.cols,
-                                                          depth.depth(),
-                                                          cameraMatrix,
-                                                          normalWinSize,
-                                                          normalMethod);
-
-                TMat c0;
-                frame->getPyramidAt(c0, OdometryFrame::PYR_CLOUD, 0);
-                normalsComputer->apply(c0, normals);
-                frame->setNormals(normals);
-            }
-        }
-        checkNormals(normals, depth.size());
-
-        preparePyramidNormals(normals, tframe->pyramids[OdometryFrame::PYR_DEPTH], tframe->pyramids[OdometryFrame::PYR_NORM]);
-
-        preparePyramidMask<TMat>(mask, tframe->pyramids[OdometryFrame::PYR_DEPTH], (float)minDepth, (float)maxDepth,
-                                 tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK]);
-
-        preparePyramidSobel<TMat>(tframe->pyramids[OdometryFrame::PYR_IMAGE], 1, 0, tframe->pyramids[OdometryFrame::PYR_DIX]);
-        preparePyramidSobel<TMat>(tframe->pyramids[OdometryFrame::PYR_IMAGE], 0, 1, tframe->pyramids[OdometryFrame::PYR_DIY]);
-        preparePyramidTexturedMask(tframe->pyramids[OdometryFrame::PYR_DIX], tframe->pyramids[OdometryFrame::PYR_DIY],
-                                   minGradientMagnitudes, tframe->pyramids[OdometryFrame::PYR_MASK],
-                                   maxPointsPart, tframe->pyramids[OdometryFrame::PYR_TEXMASK]);
-
-        preparePyramidNormalsMask(tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK],
-                                  maxPointsPart, tframe->pyramids[OdometryFrame::PYR_NORMMASK]);
-    }
-    else
-        preparePyramidMask<TMat>(mask, tframe->pyramids[OdometryFrame::PYR_DEPTH], (float)minDepth, (float)maxDepth,
-                                 tframe->pyramids[OdometryFrame::PYR_NORM], tframe->pyramids[OdometryFrame::PYR_MASK]);
-
-    return image.size();
-}
-
-
-bool RgbdICPOdometryImpl::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt, InputArray initRt) const
-{
-    return RGBDICPOdometryImpl(Rt, initRt.getMat(), srcFrame, dstFrame, cameraMatrix, (float)maxDepthDiff, iterCounts,  maxTranslation, maxRotation, MERGED_ODOMETRY, transformType);
-}
-
-//
-
-// Public Odometry classes are pure abstract, therefore a sin of multiple inheritance should be forgiven
-class FastICPOdometryImpl : public OdometryImpl, public FastICPOdometry
-{
-public:
-    /** Creates FastICPOdometry object
-     * @param cameraMatrix Camera matrix
-     * @param maxDistDiff Correspondences between pixels of two given frames will be filtered out
-     *                     if their depth difference is larger than maxDepthDiff
-     * @param angleThreshold Correspondence will be filtered out
-     *                     if an angle between their normals is bigger than threshold
-     * @param sigmaDepth Depth sigma in meters for bilateral smooth
-     * @param sigmaSpatial Spatial sigma in pixels for bilateral smooth
-     * @param kernelSize Kernel size in pixels for bilateral smooth
-     * @param iterCounts Count of iterations on each pyramid level
-     * @param depthFactor pre-scale per 1 meter for input values
-     * @param truncateThreshold Threshold for depth truncation in meters
-     *        All depth values beyond this threshold will be set to zero
-     */
-    FastICPOdometryImpl(InputArray _cameraMatrix = noArray(),
-                        float _maxDistDiff = 0.1f,
-                        float _angleThreshold = (float)(30. * CV_PI / 180.),
-                        float _sigmaDepth = 0.04f,
-                        float _sigmaSpatial = 4.5f,
-                        int _kernelSize = 7,
-                        InputArray _iterCounts = noArray(),
-                        float _depthFactor = 1.f,
-                        float _truncateThreshold = 0.f) :
-        OdometryImpl(_cameraMatrix, _iterCounts, noArray(), Odometry::RIGID_BODY_MOTION)
-    {
-        setMaxDistDiff(_maxDistDiff);
-        setAngleThreshold(_angleThreshold);
-        setSigmaDepth(_sigmaDepth);
-        setSigmaSpatial(_sigmaSpatial);
-        setKernelSize(_kernelSize);
-        setDepthFactor(_depthFactor);
-        setTruncateThreshold(_truncateThreshold);
-    }
-
-    virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const CV_OVERRIDE;
-
-    virtual Ptr<OdometryFrame> makeOdometryFrame(InputArray image, InputArray depth, InputArray mask) const CV_OVERRIDE;
-
-    virtual double getMaxDistDiff() const CV_OVERRIDE
-    {
-        return maxDistDiff;
-    }
-    virtual void setMaxDistDiff(float val) CV_OVERRIDE
-    {
-        CV_Assert(val > 0);
-        maxDistDiff = val;
-    }
-    virtual float getAngleThreshold() const CV_OVERRIDE
-    {
-        return angleThreshold;
-    }
-    virtual void setAngleThreshold(float f) CV_OVERRIDE
-    {
-        CV_Assert(f > 0);
-        angleThreshold = f;
-    }
-    virtual float getSigmaDepth() const CV_OVERRIDE
-    {
-        return sigmaDepth;
-    }
-    virtual void setSigmaDepth(float f) CV_OVERRIDE
-    {
-        CV_Assert(f > 0);
-        sigmaDepth = f;
-    }
-    virtual float getSigmaSpatial() const CV_OVERRIDE
-    {
-        return sigmaSpatial;
-    }
-    virtual void setSigmaSpatial(float f) CV_OVERRIDE
-    {
-        CV_Assert(f > 0);
-        sigmaSpatial = f;
-    }
-    virtual int getKernelSize() const CV_OVERRIDE
-    {
-        return kernelSize;
-    }
-    virtual void setKernelSize(int f) CV_OVERRIDE
-    {
-        CV_Assert(f > 0);
-        kernelSize = f;
-    }
-    virtual float getDepthFactor() const CV_OVERRIDE
-    {
-        return depthFactor;
-    }
-    virtual void setDepthFactor(float _depthFactor) CV_OVERRIDE
-    {
-        depthFactor = _depthFactor;
-    }
-    virtual float getTruncateThreshold() const CV_OVERRIDE
-    {
-        return truncateThreshold;
-    }
-    virtual void setTruncateThreshold(float _truncateThreshold) CV_OVERRIDE
-    {
-        truncateThreshold = _truncateThreshold;
-    }
-
-    virtual void getIterationCounts(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getIterationCounts(val);
-    }
-    virtual void setIterationCounts(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setIterationCounts(val);
-    }
-    virtual void getMinGradientMagnitudes(OutputArray /*val*/) const CV_OVERRIDE
-    {
-        CV_Error(Error::StsNotImplemented, "This Odometry class does not use minGradientMagnitudes");
-    }
-    virtual void setMinGradientMagnitudes(InputArray /*val*/) CV_OVERRIDE
-    {
-        CV_Error(Error::StsNotImplemented, "This Odometry class does not use minGradientMagnitudes");
-    }
-    virtual void getCameraMatrix(OutputArray val) const CV_OVERRIDE
-    {
-        OdometryImpl::getCameraMatrix(val);
-    }
-    virtual void setCameraMatrix(InputArray val) CV_OVERRIDE
-    {
-        OdometryImpl::setCameraMatrix(val);
-    }
-    virtual double getMaxTranslation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxTranslation();
-    }
-    virtual void setMaxTranslation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxTranslation(val);
-    }
-    virtual double getMaxRotation() const CV_OVERRIDE
-    {
-        return OdometryImpl::getMaxRotation();
-    }
-    virtual void setMaxRotation(double val) CV_OVERRIDE
-    {
-        OdometryImpl::setMaxRotation(val);
-    }
-    virtual Odometry::OdometryTransformType getTransformType() const CV_OVERRIDE
-    {
-        return Odometry::RIGID_BODY_MOTION;
-    }
-    virtual void setTransformType(Odometry::OdometryTransformType val) CV_OVERRIDE
-    {
-        if (val != Odometry::RIGID_BODY_MOTION)
-            CV_Error(CV_StsBadArg, "Rigid Body Motion is the only accepted transformation type for this odometry method");
-    }
-
-    virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
-                         InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcImage, srcDepth, srcMask, dstImage, dstDepth, dstMask, Rt, initRt);
-    }
-
-    virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
-                         OutputArray Rt, InputArray initRt) const CV_OVERRIDE
-    {
-        return OdometryImpl::compute(srcFrame, dstFrame, Rt, initRt);
-    }
-
-protected:
-
-    virtual bool computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
-                             InputArray initRt) const CV_OVERRIDE;
-
-    template<typename TMat>
-    Size prepareFrameCacheT(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const;
-
-    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
-    float maxDistDiff;
-
-    float angleThreshold;
-
-    float sigmaDepth;
-
-    float sigmaSpatial;
-
-    int kernelSize;
-
-    float depthFactor;
-
-    float truncateThreshold;
-};
-
-using namespace cv::kinfu;
-
-Ptr<FastICPOdometry> FastICPOdometry::create(InputArray _cameraMatrix,
-                                             float _maxDistDiff,
-                                             float _angleThreshold,
-                                             float _sigmaDepth,
-                                             float _sigmaSpatial,
-                                             int _kernelSize,
-                                             InputArray _iterCounts,
-                                             float _depthFactor,
-                                             float _truncateThreshold)
-{
-    return makePtr<FastICPOdometryImpl>(_cameraMatrix, _maxDistDiff, _angleThreshold,
-                                        _sigmaDepth, _sigmaSpatial, _kernelSize, _iterCounts,
-                                        _depthFactor, _truncateThreshold);
-}
-
-Ptr<OdometryFrame> FastICPOdometryImpl::makeOdometryFrame(InputArray _image, InputArray _depth, InputArray _mask) const
-{
-    return OdometryFrame::create(_image, _depth, _mask);
-}
-
-template<typename TMat>
-Size FastICPOdometryImpl::prepareFrameCacheT(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const
-{
-    OdometryImpl::prepareFrameCache(frame, cacheType);
-
-    TMat depth;
-    frame->getDepth(depth);
-    if (depth.empty())
-    {
-        if (frame->getPyramidLevels(OdometryFrame::PYR_CLOUD))
-        {
-            if (frame->getPyramidLevels(OdometryFrame::PYR_NORM))
-            {
-                TMat points, normals;
-                frame->getPyramidAt(points, OdometryFrame::PYR_CLOUD, 0);
-                frame->getPyramidAt(normals, OdometryFrame::PYR_NORM, 0);
-                std::vector<TMat> pyrPoints, pyrNormals;
-                // in, in, out, out
-                size_t nLevels = iterCounts.size();
-                buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, (int)nLevels);
-                for (size_t i = 1; i < nLevels; i++)
-                {
-                    frame->setPyramidAt(pyrPoints [i], OdometryFrame::PYR_CLOUD, i);
-                    frame->setPyramidAt(pyrNormals[i], OdometryFrame::PYR_NORM,  i);
-                }
-
-                return points.size();
-            }
-            else
-            {
-                TMat cloud;
-                frame->getPyramidAt(cloud, OdometryFrame::PYR_CLOUD, 0);
-                std::vector<TMat> xyz;
-                split(cloud, xyz);
-                frame->setDepth(xyz[2]);
-            }
-        }
-        else if (frame->getPyramidLevels(OdometryFrame::PYR_DEPTH))
-        {
-            TMat d0;
-            frame->getPyramidAt(d0, OdometryFrame::PYR_DEPTH, 0);
-            frame->setDepth(d0);
-        }
-        else
-            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
-    }
-    frame->getDepth(depth);
-    checkDepth(depth, depth.size());
-
-    // mask isn't used by FastICP
-    auto tframe = frame.dynamicCast<OdometryFrameImpl<TMat>>();
-    makeFrameFromDepth(depth, tframe->pyramids[OdometryFrame::PYR_CLOUD], tframe->pyramids[OdometryFrame::PYR_NORM], cameraMatrix, (int)iterCounts.size(),
-                       depthFactor, sigmaDepth, sigmaSpatial, kernelSize, truncateThreshold);
-
-    return depth.size();
-}
-
-Size FastICPOdometryImpl::prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const
-{
-    auto oclFrame = frame.dynamicCast<OdometryFrameImpl<UMat>>();
-    auto cpuFrame = frame.dynamicCast<OdometryFrameImpl<Mat>>();
-    if (oclFrame != nullptr)
-    {
-        return prepareFrameCacheT<UMat>(frame, cacheType);
-    }
-    else if (cpuFrame != nullptr)
-    {
-        return prepareFrameCacheT<Mat>(frame, cacheType);
-    }
-    else
-    {
-        CV_Error(Error::StsBadArg, "Incorrect OdometryFrame type");
-    }
-}
-
-
-bool FastICPOdometryImpl::computeImpl(const Ptr<OdometryFrame>& srcFrame,
-                                      const Ptr<OdometryFrame>& dstFrame,
-                                      OutputArray Rt, InputArray /*initRt*/) const
-{
-    Intr intr(cameraMatrix);
-    Ptr<kinfu::ICP> icp = kinfu::makeICP(intr,
-                                         iterCounts,
-                                         angleThreshold,
-                                         maxDistDiff);
-
-    // KinFu's ICP calculates transformation from new frame to old one (src to dst)
-    Affine3f transform;
-    bool result;
-    auto srcOclFrame = srcFrame.dynamicCast<OdometryFrameImpl<UMat>>();
-    auto srcCpuFrame = srcFrame.dynamicCast<OdometryFrameImpl<Mat>>();
-    auto dstOclFrame = dstFrame.dynamicCast<OdometryFrameImpl<UMat>>();
-    auto dstCpuFrame = dstFrame.dynamicCast<OdometryFrameImpl<Mat>>();
-    bool useOcl = (srcOclFrame != nullptr) && (dstOclFrame != nullptr);
-    bool useCpu = (srcCpuFrame != nullptr) && (dstCpuFrame != nullptr);
-    if (useOcl)
-    {
-        result = icp->estimateTransform(transform,
-                                        dstOclFrame->pyramids[OdometryFrame::PYR_CLOUD], dstOclFrame->pyramids[OdometryFrame::PYR_NORM],
-                                        srcOclFrame->pyramids[OdometryFrame::PYR_CLOUD], srcOclFrame->pyramids[OdometryFrame::PYR_NORM]);
-    }
-    else if (useCpu)
-    {
-        result = icp->estimateTransform(transform,
-                                        dstCpuFrame->pyramids[OdometryFrame::PYR_CLOUD], dstCpuFrame->pyramids[OdometryFrame::PYR_NORM],
-                                        srcCpuFrame->pyramids[OdometryFrame::PYR_CLOUD], srcCpuFrame->pyramids[OdometryFrame::PYR_NORM]);
-    }
-    else
-    {
-        CV_Error(Error::StsBadArg, "Both OdometryFrames should have the same storage type: Mat or UMat");
-    }
-
-    Rt.create(Size(4, 4), CV_64FC1);
-    Mat(Matx44d(transform.matrix)).copyTo(Rt.getMat());
-
-    result = result && testDeltaTransformation(Mat(Matx44d(transform.matrix)), maxTranslation, maxRotation);
-
-    return result;
-}
-
-//
-
-Ptr<Odometry> Odometry::createFromName(const std::string& odometryType)
-{
-    if (odometryType == "RgbdOdometry")
-        return RgbdOdometry::create();
-    else if (odometryType == "ICPOdometry")
-        return ICPOdometry::create();
-    else if (odometryType == "RgbdICPOdometry")
-        return RgbdICPOdometry::create();
-    else if (odometryType == "FastICPOdometry")
-        return FastICPOdometry::create();
-    return Ptr<Odometry>();
-}
-
-} // namespace cv
diff --git a/modules/3d/src/rgbd/odometry_frame_impl.cpp b/modules/3d/src/rgbd/odometry_frame_impl.cpp
new file mode 100644
index 0000000000..3ec33d6ba4
--- /dev/null
+++ b/modules/3d/src/rgbd/odometry_frame_impl.cpp
@@ -0,0 +1,251 @@
+// 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
+{
+
+class OdometryFrame::Impl
+{
+public:
+    Impl() {}
+    virtual ~Impl() {}
+    virtual void setImage(InputArray  image) = 0;
+    virtual void getImage(OutputArray image) const = 0;
+    virtual void getGrayImage(OutputArray image) const = 0;
+    virtual void setDepth(InputArray  depth) = 0;
+    virtual void getDepth(OutputArray depth) const = 0;
+    virtual void setMask(InputArray  mask) = 0;
+    virtual void getMask(OutputArray mask) const = 0;
+    virtual void setNormals(InputArray  normals) = 0;
+    virtual void getNormals(OutputArray normals) const = 0;
+    virtual void   setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype) = 0;
+    virtual void   setPyramidLevels(size_t _nLevels) = 0;
+    virtual size_t getPyramidLevels(OdometryFramePyramidType oftype) const = 0;
+    virtual void setPyramidAt(InputArray  img,
+        OdometryFramePyramidType pyrType, size_t level) = 0;
+    virtual void getPyramidAt(OutputArray img,
+        OdometryFramePyramidType pyrType, size_t level) const = 0;
+};
+
+template<typename TMat>
+class OdometryFrameImplTMat : public OdometryFrame::Impl
+{
+public:
+    OdometryFrameImplTMat();
+    ~OdometryFrameImplTMat() {};
+
+    virtual void setImage(InputArray  image) override;
+    virtual void getImage(OutputArray image) const override;
+    virtual void getGrayImage(OutputArray image) const override;
+    virtual void setDepth(InputArray  depth) override;
+    virtual void getDepth(OutputArray depth) const override;
+    virtual void setMask(InputArray  mask) override;
+    virtual void getMask(OutputArray mask) const override;
+    virtual void setNormals(InputArray  normals) override;
+    virtual void getNormals(OutputArray normals) const override;
+    virtual void   setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype) override;
+    virtual void   setPyramidLevels(size_t _nLevels) override;
+    virtual size_t getPyramidLevels(OdometryFramePyramidType oftype) const override;
+    virtual void setPyramidAt(InputArray  img,
+        OdometryFramePyramidType pyrType, size_t level) override;
+    virtual void getPyramidAt(OutputArray img,
+        OdometryFramePyramidType pyrType, size_t level) const override;
+
+private:
+    void findMask(InputArray image);
+
+    TMat image;
+    TMat imageGray;
+    TMat depth;
+    TMat mask;
+    TMat normals;
+    std::vector< std::vector<TMat> > pyramids;
+};
+
+OdometryFrame::OdometryFrame()
+{
+    this->impl = makePtr<OdometryFrameImplTMat<Mat>>();
+};
+
+OdometryFrame::OdometryFrame(OdometryFrameStoreType matType)
+{
+    if (matType == OdometryFrameStoreType::UMAT)
+        this->impl = makePtr<OdometryFrameImplTMat<UMat>>();
+    else
+        this->impl = makePtr<OdometryFrameImplTMat<Mat>>();
+};
+
+void OdometryFrame::setImage(InputArray  image) { this->impl->setImage(image); }
+void OdometryFrame::getImage(OutputArray image) const { this->impl->getImage(image); }
+void OdometryFrame::getGrayImage(OutputArray image) const { this->impl->getGrayImage(image); }
+void OdometryFrame::setDepth(InputArray  depth) { this->impl->setDepth(depth); }
+void OdometryFrame::getDepth(OutputArray depth) const { this->impl->getDepth(depth); }
+void OdometryFrame::setMask(InputArray  mask) { this->impl->setMask(mask); }
+void OdometryFrame::getMask(OutputArray mask) const { this->impl->getMask(mask); }
+void OdometryFrame::setNormals(InputArray  normals) { this->impl->setNormals(normals); }
+void OdometryFrame::getNormals(OutputArray normals) const { this->impl->getNormals(normals); }
+void OdometryFrame::setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype)
+{
+    this->impl->setPyramidLevel(_nLevels, oftype);
+}
+void OdometryFrame::setPyramidLevels(size_t _nLevels) { this->impl->setPyramidLevels(_nLevels); }
+size_t OdometryFrame::getPyramidLevels(OdometryFramePyramidType oftype) const { return this->impl->getPyramidLevels(oftype); }
+void OdometryFrame::setPyramidAt(InputArray  img, OdometryFramePyramidType pyrType, size_t level)
+{
+    this->impl->setPyramidAt(img, pyrType, level);
+}
+void OdometryFrame::getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const
+{
+    this->impl->getPyramidAt(img, pyrType, level);
+}
+
+template<typename TMat>
+OdometryFrameImplTMat<TMat>::OdometryFrameImplTMat()
+    : pyramids(OdometryFramePyramidType::N_PYRAMIDS)
+{
+};
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setImage(InputArray _image)
+{
+    this->image = getTMat<TMat>(_image);
+    Mat gray;
+    if (_image.channels() != 1)
+        cvtColor(_image, gray, COLOR_BGR2GRAY, 1);
+    else
+        gray = getTMat<Mat>(_image);
+    gray.convertTo(gray, CV_8UC1);
+    this->imageGray = getTMat<TMat>(gray);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::getImage(OutputArray _image) const
+{
+    _image.assign(this->image);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::getGrayImage(OutputArray _image) const
+{
+    _image.assign(this->imageGray);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setDepth(InputArray _depth)
+{
+
+    TMat depth_tmp, depth_flt;
+    depth_tmp = getTMat<TMat>(_depth);
+    // Odometry works with depth values in range [0, 10)
+    // if the max depth value more than 10, it needs to devide by 5000
+    double max;
+    cv::minMaxLoc(depth_tmp, NULL, &max);
+    if (max > 10)
+    {
+        depth_tmp.convertTo(depth_flt, CV_32FC1, 1.f / 5000.f);
+        depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), getTMat<Mat>(depth_flt) < FLT_EPSILON);
+        depth_tmp = depth_flt;
+    }
+    this->depth = depth_tmp;
+    this->findMask(_depth);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::getDepth(OutputArray _depth) const
+{
+    _depth.assign(this->depth);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setMask(InputArray _mask)
+{
+    this->mask = getTMat<TMat>(_mask);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::getMask(OutputArray _mask) const
+{
+    _mask.assign(this->mask);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setNormals(InputArray _normals)
+{
+    this->normals = getTMat<TMat>(_normals);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::getNormals(OutputArray _normals) const
+{
+    _normals.assign(this->normals);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setPyramidLevel(size_t _nLevels, OdometryFramePyramidType oftype)
+{
+    if (oftype < OdometryFramePyramidType::N_PYRAMIDS)
+        pyramids[oftype].resize(_nLevels, TMat());
+    else
+        CV_Error(Error::StsBadArg, "Incorrect type.");
+
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setPyramidLevels(size_t _nLevels)
+{
+    for (auto& p : pyramids)
+    {
+        p.resize(_nLevels, TMat());
+    }
+}
+
+template<typename TMat>
+size_t OdometryFrameImplTMat<TMat>::getPyramidLevels(OdometryFramePyramidType oftype) const
+{
+    if (oftype < OdometryFramePyramidType::N_PYRAMIDS)
+        return pyramids[oftype].size();
+    else
+        return 0;
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::setPyramidAt(InputArray  _img, OdometryFramePyramidType pyrType, size_t level)
+{
+    CV_Assert(pyrType >= 0);
+    CV_Assert(pyrType < pyramids.size());
+    CV_Assert(level < pyramids[pyrType].size());
+    TMat img = getTMat<TMat>(_img);
+    pyramids[pyrType][level] = img;
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::getPyramidAt(OutputArray _img, OdometryFramePyramidType pyrType, size_t level) const
+{
+    CV_Assert(pyrType < OdometryFramePyramidType::N_PYRAMIDS);
+    CV_Assert(level < pyramids[pyrType].size());
+    TMat img = pyramids[pyrType][level];
+    _img.assign(img);
+}
+
+template<typename TMat>
+void OdometryFrameImplTMat<TMat>::findMask(InputArray _depth)
+{
+    Mat d = _depth.getMat();
+    Mat m(d.size(), CV_8UC1, Scalar(255));
+    for (int y = 0; y < d.rows; y++)
+        for (int x = 0; x < d.cols; x++)
+        {
+            if (cvIsNaN(d.at<float>(y, x)) || d.at<float>(y, x) <= FLT_EPSILON)
+                m.at<uchar>(y, x) = 0;
+        }
+    this->setMask(m);
+}
+
+}
diff --git a/modules/3d/src/rgbd/odometry_functions.cpp b/modules/3d/src/rgbd/odometry_functions.cpp
new file mode 100644
index 0000000000..a120db484c
--- /dev/null
+++ b/modules/3d/src/rgbd/odometry_functions.cpp
@@ -0,0 +1,1642 @@
+// 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 "odometry_functions.hpp"
+#include "../precomp.hpp"
+#include "utils.hpp"
+#include "opencl_kernels_3d.hpp"
+
+#include "opencv2/imgproc.hpp"
+#include <opencv2/core/hal/intrin.hpp>
+#include <opencv2/core/dualquaternion.hpp>
+
+namespace cv
+{
+
+static const int normalWinSize = 5;
+static const RgbdNormals::RgbdNormalsMethod normalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
+
+enum
+{
+    UTSIZE = 27
+};
+
+void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, OdometryAlgoType algtype)
+{
+    prepareRGBFrame(srcFrame, dstFrame, settings);
+    prepareICPFrame(srcFrame, dstFrame, settings, algtype);
+}
+
+void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings)
+{
+    prepareRGBFrameBase(srcFrame, settings);
+    prepareRGBFrameBase(dstFrame, settings);
+
+    prepareRGBFrameSrc(srcFrame, settings);
+    prepareRGBFrameDst(dstFrame, settings);
+}
+
+void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, OdometryAlgoType algtype)
+{
+    prepareICPFrameBase(srcFrame, settings);
+    prepareICPFrameBase(dstFrame, settings);
+
+    prepareICPFrameSrc(srcFrame, settings);
+    if (algtype == OdometryAlgoType::FAST)
+        prepareICPFrameDst(srcFrame, settings);
+    prepareICPFrameDst(dstFrame, settings);
+}
+
+void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings)
+{
+    // Can be transformed into template argument in the future
+    // when this algorithm supports OCL UMats too
+
+    typedef Mat TMat;
+
+    TMat image;
+    frame.getGrayImage(image);
+    if (image.empty())
+    {
+        if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_IMAGE) > 0)
+        {
+            TMat pyr0;
+            frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_IMAGE, 0);
+            frame.setImage(pyr0);
+        }
+        else
+            CV_Error(Error::StsBadSize, "Image or pyramidImage have to be set.");
+    }
+    checkImage(image);
+
+
+    TMat depth;
+    frame.getDepth(depth);
+    if (depth.empty())
+    {
+        if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
+        {
+            TMat pyr0;
+            frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
+            frame.setDepth(pyr0);
+        }
+        else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
+        {
+            TMat cloud;
+            frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0);
+            std::vector<TMat> xyz;
+            split(cloud, xyz);
+            frame.setDepth(xyz[2]);
+        }
+        else
+            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
+    }
+    checkDepth(depth, image.size());
+
+    TMat mask;
+    frame.getMask(mask);
+    if (mask.empty() && frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK) > 0)
+    {
+        TMat pyr0;
+        frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_MASK, 0);
+        frame.setMask(pyr0);
+    }
+    checkMask(mask, image.size());
+
+    std::vector<int> iterCounts;
+    Mat miterCounts;
+    settings.getIterCounts(miterCounts);
+    for (int i = 0; i < miterCounts.size().height; i++)
+        iterCounts.push_back(miterCounts.at<int>(i));
+
+    std::vector<TMat> ipyramids;
+    preparePyramidImage(image, ipyramids, iterCounts.size());
+    setPyramids(frame, OdometryFramePyramidType::PYR_IMAGE, ipyramids);
+
+    std::vector<TMat> dpyramids;
+    preparePyramidImage(depth, dpyramids, iterCounts.size());
+    setPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids);
+
+    std::vector<TMat> mpyramids;
+    std::vector<TMat> npyramids;
+    preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(),
+        npyramids, mpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
+}
+
+void prepareRGBFrameSrc(OdometryFrame& frame, OdometrySettings settings)
+{
+    typedef Mat TMat;
+
+    std::vector<TMat> dpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH));
+    getPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids);
+    std::vector<TMat> mpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK));
+    getPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
+
+    std::vector<TMat> cpyramids;
+    Matx33f cameraMatrix;
+    settings.getCameraMatrix(cameraMatrix);
+
+    preparePyramidCloud<TMat>(dpyramids, cameraMatrix, cpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_CLOUD, cpyramids);
+}
+
+void prepareRGBFrameDst(OdometryFrame& frame, OdometrySettings settings)
+{
+    typedef Mat TMat;
+
+    std::vector<TMat> ipyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_IMAGE));
+    getPyramids(frame, OdometryFramePyramidType::PYR_IMAGE, ipyramids);
+    std::vector<TMat> mpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK));
+    getPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
+
+    std::vector<TMat> dxpyramids, dypyramids, tmpyramids;
+
+    Mat _minGradientMagnitudes;
+    std::vector<float> minGradientMagnitudes;
+    settings.getMinGradientMagnitudes(_minGradientMagnitudes);
+    for (int i = 0; i < _minGradientMagnitudes.size().height; i++)
+        minGradientMagnitudes.push_back(_minGradientMagnitudes.at<float>(i));
+
+    preparePyramidSobel<TMat>(ipyramids, 1, 0, dxpyramids, settings.getSobelSize());
+    preparePyramidSobel<TMat>(ipyramids, 0, 1, dypyramids, settings.getSobelSize());
+    preparePyramidTexturedMask(dxpyramids, dypyramids, minGradientMagnitudes,
+        mpyramids, settings.getMaxPointsPart(), tmpyramids, settings.getSobelScale());
+
+    setPyramids(frame, OdometryFramePyramidType::PYR_DIX, dxpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_DIY, dypyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_TEXMASK, tmpyramids);
+}
+
+void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings)
+{
+    typedef Mat TMat;
+
+    TMat depth;
+    frame.getDepth(depth);
+    if (depth.empty())
+    {
+        if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
+        {
+            TMat pyr0;
+            frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
+            frame.setDepth(pyr0);
+        }
+        else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
+        {
+            TMat cloud;
+            frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0);
+            std::vector<TMat> xyz;
+            split(cloud, xyz);
+            frame.setDepth(xyz[2]);
+        }
+        else
+            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
+    }
+    checkDepth(depth, depth.size());
+
+    TMat mask;
+    frame.getMask(mask);
+    if (mask.empty() && frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK) > 0)
+    {
+        TMat pyr0;
+        frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_MASK, 0);
+        frame.setMask(pyr0);
+    }
+    checkMask(mask, depth.size());
+
+    std::vector<int> iterCounts;
+    Mat miterCounts;
+    settings.getIterCounts(miterCounts);
+    for (int i = 0; i < miterCounts.size().height; i++)
+        iterCounts.push_back(miterCounts.at<int>(i));
+
+    std::vector<TMat> dpyramids;
+    preparePyramidImage(depth, dpyramids, iterCounts.size());
+    setPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids);
+
+    std::vector<TMat> mpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_MASK));
+    getPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
+
+    std::vector<TMat> cpyramids;
+    Matx33f cameraMatrix;
+    settings.getCameraMatrix(cameraMatrix);
+
+    preparePyramidCloud<TMat>(dpyramids, cameraMatrix, cpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_CLOUD, cpyramids);
+}
+
+void prepareICPFrameSrc(OdometryFrame& frame, OdometrySettings settings)
+{
+    typedef Mat TMat;
+
+    TMat mask;
+    frame.getMask(mask);
+
+    std::vector<TMat> dpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH));
+    getPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids);
+
+    std::vector<TMat> mpyramids;
+    std::vector<TMat> npyramids;
+    preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(),
+        npyramids, mpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
+}
+
+void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings)
+{
+    typedef Mat TMat;
+
+    Ptr<RgbdNormals> normalsComputer;
+    Matx33f cameraMatrix;
+    settings.getCameraMatrix(cameraMatrix);
+
+    TMat depth, mask, normals;
+    frame.getDepth(depth);
+    frame.getMask(mask);
+    frame.getNormals(normals);
+
+    if (normals.empty())
+    {
+        if ( frame.getPyramidLevels(OdometryFramePyramidType::PYR_NORM))
+        {
+            TMat n0;
+            frame.getPyramidAt(n0, OdometryFramePyramidType::PYR_NORM, 0);
+            frame.setNormals(n0);
+        }
+        else
+        {
+            Matx33f K;
+            if (!normalsComputer.empty())
+                normalsComputer->getK(K);
+            if (normalsComputer.empty() ||
+                normalsComputer->getRows() != depth.rows ||
+                normalsComputer->getCols() != depth.cols ||
+                norm(K, cameraMatrix) > FLT_EPSILON)
+                normalsComputer = RgbdNormals::create(depth.rows,
+                    depth.cols,
+                    depth.depth(),
+                    cameraMatrix,
+                    normalWinSize,
+                    normalMethod);
+            TMat c0;
+            frame.getPyramidAt(c0, OdometryFramePyramidType::PYR_CLOUD, 0);
+            normalsComputer->apply(c0, normals);
+            frame.setNormals(normals);
+        }
+
+    }
+
+    std::vector<TMat> npyramids;
+    std::vector<TMat> dpyramids(frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH));
+    getPyramids(frame, OdometryFramePyramidType::PYR_DEPTH, dpyramids);
+    preparePyramidNormals(normals, dpyramids, npyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_NORM, npyramids);
+
+    std::vector<TMat> mpyramids;
+    preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(),
+        npyramids, mpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
+
+    std::vector<TMat> nmpyramids;
+    preparePyramidNormalsMask(npyramids, mpyramids, settings.getMaxPointsPart(), nmpyramids);
+    setPyramids(frame, OdometryFramePyramidType::PYR_NORMMASK, nmpyramids);
+}
+
+void setPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, InputArrayOfArrays pyramidImage)
+{
+    size_t nLevels = pyramidImage.size(-1).width;
+    std::vector<Mat> pyramids;
+    pyramidImage.getMatVector(pyramids);
+    odf.setPyramidLevel(nLevels, oftype);
+    for (size_t l = 0; l < nLevels; l++)
+    {
+        odf.setPyramidAt(pyramids[l], oftype, l);
+    }
+}
+
+void getPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, OutputArrayOfArrays _pyramid)
+{
+    typedef Mat TMat;
+
+    size_t nLevels = odf.getPyramidLevels(oftype);
+    for (size_t l = 0; l < nLevels; l++)
+    {
+        TMat img;
+        odf.getPyramidAt(img, oftype, l);
+        TMat& p = _pyramid.getMatRef(int(l));
+        img.copyTo(p);
+    }
+}
+
+void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage, size_t levelCount)
+{
+    if (!pyramidImage.empty())
+    {
+        size_t nLevels = pyramidImage.size(-1).width;
+        if (nLevels < levelCount)
+            CV_Error(Error::StsBadSize, "Levels count of pyramidImage has to be equal or less than size of iterCounts.");
+
+        CV_Assert(pyramidImage.size(0) == image.size());
+        for (size_t i = 0; i < nLevels; i++)
+            CV_Assert(pyramidImage.type((int)i) == image.type());
+    }
+    else
+        buildPyramid(image, pyramidImage, (int)levelCount - 1);
+}
+
+template<typename TMat>
+void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth,
+    InputArrayOfArrays pyramidNormal,
+    InputOutputArrayOfArrays pyramidMask)
+{
+    minDepth = std::max(0.f, minDepth);
+
+    int nLevels = pyramidDepth.size(-1).width;
+    if (!pyramidMask.empty())
+    {
+        if (pyramidMask.size(-1).width != nLevels)
+            CV_Error(Error::StsBadSize, "Levels count of pyramidMask has to be equal to size of pyramidDepth.");
+
+        for (int i = 0; i < pyramidMask.size(-1).width; i++)
+        {
+            CV_Assert(pyramidMask.size(i) == pyramidDepth.size(i));
+            CV_Assert(pyramidMask.type(i) == CV_8UC1);
+        }
+    }
+    else
+    {
+        TMat validMask;
+        if (mask.empty())
+            validMask = TMat(pyramidDepth.size(0), CV_8UC1, Scalar(255));
+        else
+            validMask = getTMat<TMat>(mask, -1).clone();
+
+        buildPyramid(validMask, pyramidMask, nLevels - 1);
+
+        for (int i = 0; i < pyramidMask.size(-1).width; i++)
+        {
+            TMat levelDepth = getTMat<TMat>(pyramidDepth, i).clone();
+            patchNaNs(levelDepth, 0);
+
+            TMat& levelMask = getTMatRef<TMat>(pyramidMask, i);
+            TMat gtmin, ltmax, tmpMask;
+            cv::compare(levelDepth, Scalar(minDepth), gtmin, CMP_GT);
+            cv::compare(levelDepth, Scalar(maxDepth), ltmax, CMP_LT);
+            cv::bitwise_and(gtmin, ltmax, tmpMask);
+            cv::bitwise_and(levelMask, tmpMask, levelMask);
+
+            if (!pyramidNormal.empty())
+            {
+                CV_Assert(pyramidNormal.type(i) == CV_32FC4);
+                CV_Assert(pyramidNormal.size(i) == pyramidDepth.size(i));
+                TMat levelNormal = getTMat<TMat>(pyramidNormal, i).clone();
+
+                TMat validNormalMask;
+                // NaN check
+                cv::compare(levelNormal, levelNormal, validNormalMask, CMP_EQ);
+                CV_Assert(validNormalMask.type() == CV_8UC4);
+
+                std::vector<TMat> channelMasks;
+                split(validNormalMask, channelMasks);
+                TMat tmpChMask;
+                cv::bitwise_and(channelMasks[0], channelMasks[1], tmpChMask);
+                cv::bitwise_and(channelMasks[2], tmpChMask, validNormalMask);
+                cv::bitwise_and(levelMask, validNormalMask, levelMask);
+            }
+        }
+    }
+}
+
+template<typename TMat>
+void preparePyramidCloud(InputArrayOfArrays pyramidDepth, const Matx33f& cameraMatrix, InputOutputArrayOfArrays pyramidCloud)
+{
+    size_t depthSize = pyramidDepth.size(-1).width;
+    size_t cloudSize = pyramidCloud.size(-1).width;
+    if (!pyramidCloud.empty())
+    {
+        if (cloudSize != depthSize)
+            CV_Error(Error::StsBadSize, "Incorrect size of pyramidCloud.");
+
+        for (size_t i = 0; i < depthSize; i++)
+        {
+            CV_Assert(pyramidCloud.size((int)i) == pyramidDepth.size((int)i));
+            CV_Assert(pyramidCloud.type((int)i) == CV_32FC4);
+        }
+    }
+    else
+    {
+        std::vector<Matx33f> pyramidCameraMatrix;
+        buildPyramidCameraMatrix(cameraMatrix, (int)depthSize, pyramidCameraMatrix);
+
+        pyramidCloud.create((int)depthSize, 1, CV_32FC4, -1);
+        for (size_t i = 0; i < depthSize; i++)
+        {
+            TMat cloud;
+            depthTo3d(getTMat<TMat>(pyramidDepth, (int)i), pyramidCameraMatrix[i], cloud, Mat());
+            getTMatRef<TMat>(pyramidCloud, (int)i) = cloud;
+
+        }
+    }
+}
+
+void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector<Matx33f>& pyramidCameraMatrix)
+{
+    pyramidCameraMatrix.resize(levels);
+
+    for (int i = 0; i < levels; i++)
+    {
+        Matx33f levelCameraMatrix = (i == 0) ? cameraMatrix : 0.5f * pyramidCameraMatrix[i - 1];
+        levelCameraMatrix(2, 2) = 1.0;
+        pyramidCameraMatrix[i] = levelCameraMatrix;
+    }
+}
+
+
+template<typename TMat>
+void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel, int sobelSize)
+{
+    size_t imgLevels = pyramidImage.size(-1).width;
+    size_t sobelLvls = pyramidSobel.size(-1).width;
+    if (!pyramidSobel.empty())
+    {
+        if (sobelLvls != imgLevels)
+            CV_Error(Error::StsBadSize, "Incorrect size of pyramidSobel.");
+
+        for (size_t i = 0; i < sobelLvls; i++)
+        {
+            CV_Assert(pyramidSobel.size((int)i) == pyramidImage.size((int)i));
+            CV_Assert(pyramidSobel.type((int)i) == CV_16SC1);
+        }
+    }
+    else
+    {
+        pyramidSobel.create((int)imgLevels, 1, CV_16SC1, -1);
+        for (size_t i = 0; i < imgLevels; i++)
+        {
+            Sobel(getTMat<TMat>(pyramidImage, (int)i), getTMatRef<TMat>(pyramidSobel, (int)i), CV_16S, dx, dy, sobelSize);
+        }
+    }
+}
+
+void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy,
+    InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
+    InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale)
+{
+    size_t didxLevels = pyramid_dI_dx.size(-1).width;
+    size_t texLevels = pyramidTexturedMask.size(-1).width;
+    if (!pyramidTexturedMask.empty())
+    {
+        if (texLevels != didxLevels)
+            CV_Error(Error::StsBadSize, "Incorrect size of pyramidTexturedMask.");
+
+        for (size_t i = 0; i < texLevels; i++)
+        {
+            CV_Assert(pyramidTexturedMask.size((int)i) == pyramid_dI_dx.size((int)i));
+            CV_Assert(pyramidTexturedMask.type((int)i) == CV_8UC1);
+        }
+    }
+    else
+    {
+        CV_Assert(minGradMagnitudes.type() == CV_32F);
+        Mat_<float> mgMags = minGradMagnitudes.getMat();
+
+        const float sobelScale2_inv = (float) (1. / (sobelScale * sobelScale));
+        pyramidTexturedMask.create((int)didxLevels, 1, CV_8UC1, -1);
+        for (size_t i = 0; i < didxLevels; i++)
+        {
+            const float minScaledGradMagnitude2 = mgMags((int)i) * mgMags((int)i) * sobelScale2_inv;
+            const Mat& dIdx = pyramid_dI_dx.getMat((int)i);
+            const Mat& dIdy = pyramid_dI_dy.getMat((int)i);
+
+            Mat texturedMask(dIdx.size(), CV_8UC1, Scalar(0));
+
+            for (int y = 0; y < dIdx.rows; y++)
+            {
+                const short* dIdx_row = dIdx.ptr<short>(y);
+                const short* dIdy_row = dIdy.ptr<short>(y);
+                uchar* texturedMask_row = texturedMask.ptr<uchar>(y);
+                for (int x = 0; x < dIdx.cols; x++)
+                {
+                    float magnitude2 = static_cast<float>(dIdx_row[x] * dIdx_row[x] + dIdy_row[x] * dIdy_row[x]);
+                    if (magnitude2 >= minScaledGradMagnitude2)
+                        texturedMask_row[x] = 255;
+                }
+            }
+            Mat texMask = texturedMask & pyramidMask.getMat((int)i);
+
+            randomSubsetOfMask(texMask, (float)maxPointsPart);
+            pyramidTexturedMask.getMatRef((int)i) = texMask;
+        }
+    }
+}
+
+void randomSubsetOfMask(InputOutputArray _mask, float part)
+{
+    const int minPointsCount = 1000; // minimum point count (we can process them fast)
+    const int nonzeros = countNonZero(_mask);
+    const int needCount = std::max(minPointsCount, int(_mask.total() * part));
+    if (needCount < nonzeros)
+    {
+        RNG rng;
+        Mat mask = _mask.getMat();
+        Mat subset(mask.size(), CV_8UC1, Scalar(0));
+
+        int subsetSize = 0;
+        while (subsetSize < needCount)
+        {
+            int y = rng(mask.rows);
+            int x = rng(mask.cols);
+            if (mask.at<uchar>(y, x))
+            {
+                subset.at<uchar>(y, x) = 255;
+                mask.at<uchar>(y, x) = 0;
+                subsetSize++;
+            }
+        }
+        _mask.assign(subset);
+    }
+}
+
+void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals)
+{
+    size_t depthLevels = pyramidDepth.size(-1).width;
+    size_t normalsLevels = pyramidNormals.size(-1).width;
+    if (!pyramidNormals.empty())
+    {
+        if (normalsLevels != depthLevels)
+            CV_Error(Error::StsBadSize, "Incorrect size of pyramidNormals.");
+
+        for (size_t i = 0; i < normalsLevels; i++)
+        {
+            CV_Assert(pyramidNormals.size((int)i) == pyramidDepth.size((int)i));
+            CV_Assert(pyramidNormals.type((int)i) == CV_32FC3);
+        }
+    }
+    else
+    {
+        buildPyramid(normals, pyramidNormals, (int)depthLevels - 1);
+        // renormalize normals
+        for (size_t i = 1; i < depthLevels; i++)
+        {
+            Mat& currNormals = pyramidNormals.getMatRef((int)i);
+            for (int y = 0; y < currNormals.rows; y++)
+            {
+                Point3f* normals_row = currNormals.ptr<Point3f>(y);
+                for (int x = 0; x < currNormals.cols; x++)
+                {
+                    double nrm = norm(normals_row[x]);
+                    normals_row[x] *= 1. / nrm;
+                }
+            }
+        }
+    }
+}
+
+void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart,
+    InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask)
+{
+    size_t maskLevels = pyramidMask.size(-1).width;
+    size_t norMaskLevels = pyramidNormalsMask.size(-1).width;
+    if (!pyramidNormalsMask.empty())
+    {
+        if (norMaskLevels != maskLevels)
+            CV_Error(Error::StsBadSize, "Incorrect size of pyramidNormalsMask.");
+
+        for (size_t i = 0; i < norMaskLevels; i++)
+        {
+            CV_Assert(pyramidNormalsMask.size((int)i) == pyramidMask.size((int)i));
+            CV_Assert(pyramidNormalsMask.type((int)i) == pyramidMask.type((int)i));
+        }
+    }
+    else
+    {
+        pyramidNormalsMask.create((int)maskLevels, 1, CV_8U, -1);
+        for (size_t i = 0; i < maskLevels; i++)
+        {
+            Mat& normalsMask = pyramidNormalsMask.getMatRef((int)i);
+            normalsMask = pyramidMask.getMat((int)i).clone();
+
+            const Mat normals = pyramidNormals.getMat((int)i);
+            for (int y = 0; y < normalsMask.rows; y++)
+            {
+                const Vec4f* normals_row = normals.ptr<Vec4f>(y);
+                uchar* normalsMask_row = normalsMask.ptr<uchar>(y);
+                for (int x = 0; x < normalsMask.cols; x++)
+                {
+                    Vec4f n = normals_row[x];
+                    if (cvIsNaN(n[0]))
+                    {
+                        normalsMask_row[x] = 0;
+                    }
+                }
+            }
+            randomSubsetOfMask(normalsMask, (float)maxPointsPart);
+        }
+    }
+}
+
+
+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)
+{
+    int transformDim = -1;
+    CalcRgbdEquationCoeffsPtr rgbdEquationFuncPtr = 0;
+    CalcICPEquationCoeffsPtr icpEquationFuncPtr = 0;
+    switch(transfromType)
+    {
+    case OdometryTransformType::RIGID_TRANSFORMATION:
+        transformDim = 6;
+        rgbdEquationFuncPtr = calcRgbdEquationCoeffs;
+        icpEquationFuncPtr = calcICPEquationCoeffs;
+        break;
+    case OdometryTransformType::ROTATION:
+        transformDim = 3;
+        rgbdEquationFuncPtr = calcRgbdEquationCoeffsRotation;
+        icpEquationFuncPtr = calcICPEquationCoeffsRotation;
+        break;
+    case OdometryTransformType::TRANSLATION:
+        transformDim = 3;
+        rgbdEquationFuncPtr = calcRgbdEquationCoeffsTranslation;
+        icpEquationFuncPtr = calcICPEquationCoeffsTranslation;
+        break;
+    default:
+        CV_Error(Error::StsBadArg, "Incorrect transformation type");
+    }
+
+    const int minOverdetermScale = 20;
+    const int minCorrespsCount = minOverdetermScale * transformDim;
+
+    std::vector<Matx33f> pyramidCameraMatrix;
+    buildPyramidCameraMatrix(cameraMatrix, (int)iterCounts.size(), pyramidCameraMatrix);
+
+    Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
+    Mat currRt, ksi;
+    Affine3f transform = Affine3f::Identity();
+
+    bool isOk = false;
+    for(int level = (int)iterCounts.size() - 1; level >= 0; level--)
+    {
+        const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level];
+        const Matx33f& levelCameraMatrix_inv = levelCameraMatrix.inv(DECOMP_SVD);
+        const Mat srcLevelDepth, dstLevelDepth;
+        const Mat srcLevelImage, dstLevelImage;
+        srcFrame.getPyramidAt(srcLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level);
+        dstFrame.getPyramidAt(dstLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level);
+
+        if (method != OdometryType::DEPTH)
+        {
+            srcFrame.getPyramidAt(srcLevelImage, OdometryFramePyramidType::PYR_IMAGE, level);
+            dstFrame.getPyramidAt(dstLevelImage, OdometryFramePyramidType::PYR_IMAGE, level);
+        }
+
+        const double fx = levelCameraMatrix(0, 0);
+        const double fy = levelCameraMatrix(1, 1);
+        const double determinantThreshold = 1e-6;
+
+        Mat AtA_rgbd, AtB_rgbd, AtA_icp, AtB_icp;
+
+        // Run transformation search on current level iteratively.
+        for(int iter = 0; iter < iterCounts[level]; iter ++)
+        {
+            Mat resultRt_inv = resultRt.inv(DECOMP_SVD);
+            Mat corresps_rgbd, corresps_icp, diffs_rgbd, diffs_icp;
+            double sigma_rgbd = 0, sigma_icp = 0;
+
+            const Mat pyramidMask;
+            srcFrame.getPyramidAt(pyramidMask, OdometryFramePyramidType::PYR_MASK, level);
+
+            if(method != OdometryType::DEPTH)// RGB
+            {
+                const Mat pyramidTexturedMask;
+                dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level);
+                computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
+                                srcLevelImage, srcLevelDepth, pyramidMask,
+                                dstLevelImage, dstLevelDepth, pyramidTexturedMask, maxDepthDiff,
+                                corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB);
+            }
+
+            if(method != OdometryType::RGB)// ICP
+            {
+                if (algtype == OdometryAlgoType::COMMON)
+                {
+                    const Mat pyramidNormalsMask;
+                    dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level);
+                    computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
+                        Mat(), srcLevelDepth, pyramidMask,
+                        Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff,
+                        corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH);
+                }
+            }
+
+            if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount && algtype != OdometryAlgoType::FAST)
+                break;
+
+            const Mat srcPyrCloud;
+            srcFrame.getPyramidAt(srcPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level);
+
+
+            Mat AtA(transformDim, transformDim, CV_64FC1, Scalar(0)), AtB(transformDim, 1, CV_64FC1, Scalar(0));
+            if(corresps_rgbd.rows >= minCorrespsCount)
+            {
+                const Mat srcPyrImage, dstPyrImage, dstPyrIdx, dstPyrIdy;
+                srcFrame.getPyramidAt(srcPyrImage, OdometryFramePyramidType::PYR_IMAGE, level);
+                dstFrame.getPyramidAt(dstPyrImage, OdometryFramePyramidType::PYR_IMAGE, level);
+                dstFrame.getPyramidAt(dstPyrIdx, OdometryFramePyramidType::PYR_DIX, level);
+                dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level);
+                calcRgbdLsmMatrices(srcPyrCloud, resultRt, dstPyrIdx, dstPyrIdy,
+                                    corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale,
+                                    AtA_rgbd, AtB_rgbd, rgbdEquationFuncPtr, transformDim);
+                AtA += AtA_rgbd;
+                AtB += AtB_rgbd;
+            }
+            if(corresps_icp.rows >= minCorrespsCount || algtype == OdometryAlgoType::FAST)
+            {
+                const Mat dstPyrCloud, dstPyrNormals, srcPyrNormals;
+                dstFrame.getPyramidAt(dstPyrCloud, OdometryFramePyramidType::PYR_CLOUD, level);
+                dstFrame.getPyramidAt(dstPyrNormals, OdometryFramePyramidType::PYR_NORM, level);
+
+                if (algtype == OdometryAlgoType::COMMON)
+                {
+                    calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals,
+                        corresps_icp, AtA_icp, AtB_icp, icpEquationFuncPtr, transformDim);
+                }
+                else
+                {
+                    srcFrame.getPyramidAt(srcPyrNormals, OdometryFramePyramidType::PYR_NORM, level);
+                    cv::Matx66f A;
+                    cv::Vec6f b;
+                    calcICPLsmMatricesFast(cameraMatrix, dstPyrCloud, dstPyrNormals, srcPyrCloud, srcPyrNormals, transform, level, maxDepthDiff, angleThreshold, A, b);
+                    AtA_icp = Mat(A);
+                    AtB_icp = Mat(b);
+                }
+
+                AtA += AtA_icp;
+                AtB += AtB_icp;
+            }
+
+            bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi);
+
+            if (!solutionExist)
+            {
+                break;
+            }
+            if(transfromType == OdometryTransformType::ROTATION)
+            {
+                Mat tmp(6, 1, CV_64FC1, Scalar(0));
+                ksi.copyTo(tmp.rowRange(0,3));
+                ksi = tmp;
+            }
+            else if(transfromType == OdometryTransformType::TRANSLATION)
+            {
+                Mat tmp(6, 1, CV_64FC1, Scalar(0));
+                ksi.copyTo(tmp.rowRange(3,6));
+                ksi = tmp;
+            }
+
+            computeProjectiveMatrix(ksi, currRt);
+            resultRt = currRt * resultRt;
+            Vec6f x(ksi);
+            Affine3f tinc(Vec3f(x.val), Vec3f(x.val + 3));
+            transform = tinc * transform;
+
+            isOk = true;
+
+        }
+
+    }
+
+    _Rt.create(resultRt.size(), resultRt.type());
+    Mat Rt = _Rt.getMat();
+    resultRt.copyTo(Rt);
+
+    if(isOk)
+    {
+        Mat deltaRt;
+        if(initRt.empty())
+            deltaRt = resultRt;
+        else
+            deltaRt = resultRt * initRt.inv(DECOMP_SVD);
+
+        isOk = testDeltaTransformation(deltaRt, maxTranslation, maxRotation);
+    }
+
+    return isOk;
+}
+
+// In RGB case compute sigma and diffs too
+void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, 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)
+{
+    CV_Assert(Rt.type() == CV_64FC1);
+
+    Mat corresps(depth1.size(), CV_16SC2, Scalar::all(-1));
+    Mat diffs(depth1.size(), CV_32F, Scalar::all(-1));
+
+    Matx33d K(_K), K_inv(_K_inv);
+    Rect r(0, 0, depth1.cols, depth1.rows);
+    Mat Kt = Rt(Rect(3, 0, 1, 3)).clone();
+    Kt = K * Kt;
+    const double* Kt_ptr = Kt.ptr<const double>();
+
+    AutoBuffer<float> buf(3 * (depth1.cols + depth1.rows));
+    float* KRK_inv0_u1 = buf.data();
+    float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depth1.cols;
+    float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depth1.rows;
+    float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depth1.cols;
+    float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depth1.rows;
+    float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depth1.cols;
+    {
+        Mat R = Rt(Rect(0, 0, 3, 3)).clone();
+
+        Mat KRK_inv = K * R * K_inv;
+        const double* KRK_inv_ptr = KRK_inv.ptr<const double>();
+        for (int u1 = 0; u1 < depth1.cols; u1++)
+        {
+            KRK_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1);
+            KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1);
+            KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * u1);
+        }
+
+        for (int v1 = 0; v1 < depth1.rows; v1++)
+        {
+            KRK_inv1_v1_plus_KRK_inv2[v1] = (float)(KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]);
+            KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]);
+            KRK_inv7_v1_plus_KRK_inv8[v1] = (float)(KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]);
+        }
+    }
+
+    double sigma = 0;
+    int correspCount = 0;
+    for (int v1 = 0; v1 < depth1.rows; v1++)
+    {
+        const float* depth1_row = depth1.ptr<float>(v1);
+        const uchar* mask1_row = selectMask1.ptr<uchar>(v1);
+        for (int u1 = 0; u1 < depth1.cols; u1++)
+        {
+            float d1 = depth1_row[u1];
+            if (mask1_row[u1])
+            {
+                CV_DbgAssert(!cvIsNaN(d1));
+                float transformed_d1 = static_cast<float>(d1 * (KRK_inv6_u1[u1] + KRK_inv7_v1_plus_KRK_inv8[v1]) +
+                    Kt_ptr[2]);
+
+                if (transformed_d1 > 0)
+                {
+                    float transformed_d1_inv = 1.f / transformed_d1;
+                    int u0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv0_u1[u1] + KRK_inv1_v1_plus_KRK_inv2[v1]) +
+                        Kt_ptr[0]));
+                    int v0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv3_u1[u1] + KRK_inv4_v1_plus_KRK_inv5[v1]) +
+                        Kt_ptr[1]));
+                    if (r.contains(Point(u0, v0)))
+                    {
+                        float d0 = depth0.at<float>(v0, u0);
+                        if (validMask0.at<uchar>(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff)
+                        {
+                            CV_DbgAssert(!cvIsNaN(d0));
+                            Vec2s& c = corresps.at<Vec2s>(v0, u0);
+                            float& d = diffs.at<float>(v0, u0);
+                            float diff = 0;
+                            if (c[0] != -1)
+                            {
+                                diff = 0;
+                                int exist_u1 = c[0], exist_v1 = c[1];
+
+                                float exist_d1 = (float)(depth1.at<float>(exist_v1, exist_u1) *
+                                    (KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + Kt_ptr[2]);
+
+                                if (transformed_d1 > exist_d1)
+                                    continue;
+                                if (method == OdometryType::RGB)
+                                    diff = static_cast<float>(static_cast<int>(image0.at<uchar>(v0, u0)) -
+                                        static_cast<int>(image1.at<uchar>(v1, u1)));
+                            }
+                            else
+                            {
+                                if (method == OdometryType::RGB)
+                                    diff = static_cast<float>(static_cast<int>(image0.at<uchar>(v0, u0)) -
+                                                              static_cast<int>(image1.at<uchar>(v1, u1)));
+                                correspCount++;
+                            }
+                            c = Vec2s((short)u1, (short)v1);
+                            d = diff;
+                            sigma += diff * diff;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    _sigma = std::sqrt(sigma / double(correspCount));
+
+    _corresps.create(correspCount, 1, CV_32SC4);
+    _diffs.create(correspCount, 1, CV_32F);
+    Vec4i* corresps_ptr = _corresps.ptr<Vec4i>();
+    float* diffs_ptr = _diffs.ptr<float>();
+    for (int v0 = 0, i = 0; v0 < corresps.rows; v0++)
+    {
+        const Vec2s* corresps_row = corresps.ptr<Vec2s>(v0);
+        const float* diffs_row = diffs.ptr<float>(v0);
+        for (int u0 = 0; u0 < corresps.cols; u0++)
+        {
+            const Vec2s& c = corresps_row[u0];
+            const float& d = diffs_row[u0];
+            if (c[0] != -1)
+            {
+                corresps_ptr[i] = Vec4i(u0, v0, c[0], c[1]);
+                if (method == OdometryType::RGB)
+                    diffs_ptr[i] = d;
+                i++;
+            }
+        }
+    }
+}
+
+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, CalcRgbdEquationCoeffsPtr func, int transformDim)
+{
+    AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
+    AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
+    double* AtB_ptr = AtB.ptr<double>();
+
+
+    CV_Assert(Rt.type() == CV_64FC1);
+    const double* Rt_ptr = Rt.ptr<const double>();
+
+    const float* diffs_ptr = _diffs.ptr<float>();
+    const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
+    double sigma = _sigma;
+
+    std::vector<double> A_buf(transformDim);
+    double* A_ptr = &A_buf[0];
+
+    for (int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
+    {
+        const Vec4i& c = corresps_ptr[correspIndex];
+        int u0 = c[0], v0 = c[1];
+        int u1 = c[2], v1 = c[3];
+
+        double w = sigma + std::abs(diffs_ptr[correspIndex]);
+        w = w > DBL_EPSILON ? 1. / w : 1.;
+
+        double w_sobelScale = w * sobelScaleIn;
+
+        const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
+        Point3f tp0;
+        tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]);
+        tp0.y = (float)(p0[0] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]);
+        tp0.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]);
+
+        func(A_ptr,
+            w_sobelScale * dI_dx1.at<short int>(v1, u1),
+            w_sobelScale * dI_dy1.at<short int>(v1, u1),
+            tp0, fx, fy);
+
+        for (int y = 0; y < transformDim; y++)
+        {
+            double* AtA_ptr = AtA.ptr<double>(y);
+            for (int x = y; x < transformDim; x++)
+            {
+                AtA_ptr[x] += A_ptr[y] * A_ptr[x];
+            }
+            AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
+        }
+    }
+
+    for (int y = 0; y < transformDim; y++)
+        for (int x = y + 1; x < transformDim; x++)
+            AtA.at<double>(x, y) = AtA.at<double>(y, x);
+}
+
+void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
+    const Mat& cloud1, const Mat& normals1,
+    const Mat& corresps,
+    Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim)
+{
+    AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0));
+    AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0));
+    double* AtB_ptr = AtB.ptr<double>();
+
+    const int correspsCount = corresps.rows;
+
+    CV_Assert(Rt.type() == CV_64FC1);
+    const double* Rt_ptr = Rt.ptr<const double>();
+
+    AutoBuffer<float> diffs(correspsCount);
+    float* diffs_ptr = diffs.data();
+
+    AutoBuffer<Point3f> transformedPoints0(correspsCount);
+    Point3f* tps0_ptr = transformedPoints0.data();
+
+    const Vec4i* corresps_ptr = corresps.ptr<Vec4i>();
+
+    double sigma = 0;
+    for (int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
+    {
+        const Vec4i& c = corresps_ptr[correspIndex];
+        int u0 = c[0], v0 = c[1];
+        int u1 = c[2], v1 = c[3];
+
+        const Vec4f& p0 = cloud0.at<Vec4f>(v0, u0);
+        Point3f tp0;
+        tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]);
+        tp0.y = (float)(p0[0] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]);
+        tp0.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]);
+
+        Vec4f n1 = normals1.at<Vec4f>(v1, u1);
+        Vec4f _v = cloud1.at<Vec4f>(v1, u1);
+        Point3f v = Point3f(_v[0], _v[1], _v[2]) - tp0;
+
+        tps0_ptr[correspIndex] = tp0;
+        diffs_ptr[correspIndex] = n1[0] * v.x + n1[1] * v.y + n1[2] * v.z;
+        sigma += diffs_ptr[correspIndex] * diffs_ptr[correspIndex];
+    }
+
+    sigma = std::sqrt(sigma / correspsCount);
+
+    std::vector<double> A_buf(transformDim);
+    double* A_ptr = &A_buf[0];
+    for (int correspIndex = 0; correspIndex < corresps.rows; correspIndex++)
+    {
+        const Vec4i& c = corresps_ptr[correspIndex];
+        int u1 = c[2], v1 = c[3];
+
+        double w = sigma + std::abs(diffs_ptr[correspIndex]);
+        w = w > DBL_EPSILON ? 1. / w : 1.;
+
+        Vec4f n4 = normals1.at<Vec4f>(v1, u1);
+        func(A_ptr, tps0_ptr[correspIndex], Vec3f(n4[0], n4[1], n4[2]) * w);
+
+        for (int y = 0; y < transformDim; y++)
+        {
+            double* AtA_ptr = AtA.ptr<double>(y);
+            for (int x = y; x < transformDim; x++)
+                AtA_ptr[x] += A_ptr[y] * A_ptr[x];
+
+            AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex];
+        }
+    }
+
+    for (int y = 0; y < transformDim; y++)
+        for (int x = y + 1; x < transformDim; x++)
+            AtA.at<double>(x, y) = AtA.at<double>(y, x);
+}
+
+void computeProjectiveMatrix(const Mat& ksi, Mat& Rt)
+{
+    CV_Assert(ksi.size() == Size(1, 6) && ksi.type() == CV_64FC1);
+
+    const double* ksi_ptr = ksi.ptr<const double>();
+    // 0.5 multiplication is here because (dual) quaternions keep half an angle/twist inside
+    Matx44d matdq = (DualQuatd(0, ksi_ptr[0], ksi_ptr[1], ksi_ptr[2],
+        0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5]) * 0.5).exp().toMat(QUAT_ASSUME_UNIT);
+    Mat(matdq).copyTo(Rt);
+}
+
+bool solveSystem(const Mat& AtA, const Mat& AtB, double detThreshold, Mat& x)
+{
+    double det = determinant(AtA);
+    if (fabs(det) < detThreshold || cvIsNaN(det) || cvIsInf(det))
+        return false;
+
+    solve(AtA, AtB, x, DECOMP_CHOLESKY);
+
+    return true;
+}
+
+bool testDeltaTransformation(const Mat& deltaRt, double maxTranslation, double maxRotation)
+{
+    double translation = norm(deltaRt(Rect(3, 0, 1, 3)));
+
+    Mat rvec;
+    Rodrigues(deltaRt(Rect(0, 0, 3, 3)), rvec);
+
+    double rotation = norm(rvec) * 180. / CV_PI;
+
+    return translation <= maxTranslation && rotation <= maxRotation;
+}
+
+#if USE_INTRINSICS
+static inline bool fastCheck(const v_float32x4& p0, const v_float32x4& p1)
+{
+    float check = v_reduce_sum(p0) + v_reduce_sum(p1);
+    return !cvIsNaN(check);
+}
+
+static inline void getCrossPerm(const v_float32x4& a, v_float32x4& yzx, v_float32x4& zxy)
+{
+    v_uint32x4 aa = v_reinterpret_as_u32(a);
+    v_uint32x4 yz00 = v_extract<1>(aa, v_setzero_u32());
+    v_uint32x4 x0y0, tmp;
+    v_zip(aa, v_setzero_u32(), x0y0, tmp);
+    v_uint32x4 yzx0 = v_combine_low(yz00, x0y0);
+    v_uint32x4 y000 = v_extract<2>(x0y0, v_setzero_u32());
+    v_uint32x4 zx00 = v_extract<1>(yzx0, v_setzero_u32());
+    zxy = v_reinterpret_as_f32(v_combine_low(zx00, y000));
+    yzx = v_reinterpret_as_f32(yzx0);
+}
+
+static inline v_float32x4 crossProduct(const v_float32x4& a, const v_float32x4& b)
+{
+    v_float32x4 ayzx, azxy, byzx, bzxy;
+    getCrossPerm(a, ayzx, azxy);
+    getCrossPerm(b, byzx, bzxy);
+    return ayzx * bzxy - azxy * byzx;
+}
+#else
+static inline bool fastCheck(const Point3f& p)
+{
+    return !cvIsNaN(p.x);
+}
+
+#endif
+
+typedef Matx<float, 6, 7> ABtype;
+
+struct GetAbInvoker : ParallelLoopBody
+{
+    GetAbInvoker(ABtype& _globalAb, Mutex& _mtx,
+        const Points& _oldPts, const Normals& _oldNrm, const Points& _newPts, const Normals& _newNrm,
+        Affine3f _pose, Intr::Projector _proj, float _sqDistanceThresh, float _minCos) :
+        ParallelLoopBody(),
+        globalSumAb(_globalAb), mtx(_mtx),
+        oldPts(_oldPts), oldNrm(_oldNrm), newPts(_newPts), newNrm(_newNrm), pose(_pose),
+        proj(_proj), sqDistanceThresh(_sqDistanceThresh), minCos(_minCos)
+    { }
+
+    virtual void operator ()(const Range& range) const override
+    {
+#if USE_INTRINSICS
+        CV_Assert(ptype::channels == 4);
+
+        const size_t utBufferSize = 9;
+        float CV_DECL_ALIGNED(16) upperTriangle[utBufferSize * 4];
+        for (size_t i = 0; i < utBufferSize * 4; i++)
+            upperTriangle[i] = 0;
+        // how values are kept in upperTriangle
+        const int NA = 0;
+        const size_t utPos[] =
+        {
+           0,  1,  2,  4,  5,  6,  3,
+          NA,  9, 10, 12, 13, 14, 11,
+          NA, NA, 18, 20, 21, 22, 19,
+          NA, NA, NA, 24, 28, 30, 32,
+          NA, NA, NA, NA, 25, 29, 33,
+          NA, NA, NA, NA, NA, 26, 34
+        };
+
+        const float(&pm)[16] = pose.matrix.val;
+        v_float32x4 poseRot0(pm[0], pm[4], pm[8], 0);
+        v_float32x4 poseRot1(pm[1], pm[5], pm[9], 0);
+        v_float32x4 poseRot2(pm[2], pm[6], pm[10], 0);
+        v_float32x4 poseTrans(pm[3], pm[7], pm[11], 0);
+
+        v_float32x4 vfxy(proj.fx, proj.fy, 0, 0), vcxy(proj.cx, proj.cy, 0, 0);
+        v_float32x4 vframe((float)(oldPts.cols - 1), (float)(oldPts.rows - 1), 1.f, 1.f);
+
+        float sqThresh = sqDistanceThresh;
+        float cosThresh = minCos;
+
+        for (int y = range.start; y < range.end; y++)
+        {
+            const CV_DECL_ALIGNED(16) float* newPtsRow = (const float*)newPts[y];
+            const CV_DECL_ALIGNED(16) float* newNrmRow = (const float*)newNrm[y];
+
+            for (int x = 0; x < newPts.cols; x++)
+            {
+                v_float32x4 newP = v_load_aligned(newPtsRow + x * 4);
+                v_float32x4 newN = v_load_aligned(newNrmRow + x * 4);
+
+                if (!fastCheck(newP, newN))
+                    continue;
+
+                //transform to old coord system
+                newP = v_matmuladd(newP, poseRot0, poseRot1, poseRot2, poseTrans);
+                newN = v_matmuladd(newN, poseRot0, poseRot1, poseRot2, v_setzero_f32());
+
+                //find correspondence by projecting the point
+                v_float32x4 oldCoords;
+                float pz = (v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(newP))).get0());
+                // x, y, 0, 0
+                oldCoords = v_muladd(newP / v_setall_f32(pz), vfxy, vcxy);
+
+                if (!v_check_all((oldCoords >= v_setzero_f32()) & (oldCoords < vframe)))
+                    continue;
+
+                // bilinearly interpolate oldPts and oldNrm under oldCoords point
+                v_float32x4 oldP;
+                v_float32x4 oldN;
+                {
+                    v_int32x4 ixy = v_floor(oldCoords);
+                    v_float32x4 txy = oldCoords - v_cvt_f32(ixy);
+                    int xi = ixy.get0();
+                    int yi = v_rotate_right<1>(ixy).get0();
+                    v_float32x4 tx = v_setall_f32(txy.get0());
+                    txy = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(txy)));
+                    v_float32x4 ty = v_setall_f32(txy.get0());
+
+                    const float* prow0 = (const float*)oldPts[yi + 0];
+                    const float* prow1 = (const float*)oldPts[yi + 1];
+
+                    v_float32x4 p00 = v_load(prow0 + (xi + 0) * 4);
+                    v_float32x4 p01 = v_load(prow0 + (xi + 1) * 4);
+                    v_float32x4 p10 = v_load(prow1 + (xi + 0) * 4);
+                    v_float32x4 p11 = v_load(prow1 + (xi + 1) * 4);
+
+                    // do not fix missing data
+                    // NaN check is done later
+
+                    const float* nrow0 = (const float*)oldNrm[yi + 0];
+                    const float* nrow1 = (const float*)oldNrm[yi + 1];
+
+                    v_float32x4 n00 = v_load(nrow0 + (xi + 0) * 4);
+                    v_float32x4 n01 = v_load(nrow0 + (xi + 1) * 4);
+                    v_float32x4 n10 = v_load(nrow1 + (xi + 0) * 4);
+                    v_float32x4 n11 = v_load(nrow1 + (xi + 1) * 4);
+
+                    // NaN check is done later
+
+                    v_float32x4 p0 = p00 + tx * (p01 - p00);
+                    v_float32x4 p1 = p10 + tx * (p11 - p10);
+                    oldP = p0 + ty * (p1 - p0);
+
+                    v_float32x4 n0 = n00 + tx * (n01 - n00);
+                    v_float32x4 n1 = n10 + tx * (n11 - n10);
+                    oldN = n0 + ty * (n1 - n0);
+                }
+
+                bool oldPNcheck = fastCheck(oldP, oldN);
+
+                //filter by distance
+                v_float32x4 diff = newP - oldP;
+                bool distCheck = !(v_reduce_sum(diff * diff) > sqThresh);
+
+                //filter by angle
+                bool angleCheck = !(abs(v_reduce_sum(newN * oldN)) < cosThresh);
+
+                if (!(oldPNcheck && distCheck && angleCheck))
+                    continue;
+
+                // build point-wise vector ab = [ A | b ]
+                v_float32x4 VxNv = crossProduct(newP, oldN);
+                Point3f VxN;
+                VxN.x = VxNv.get0();
+                VxN.y = v_reinterpret_as_f32(v_extract<1>(v_reinterpret_as_u32(VxNv), v_setzero_u32())).get0();
+                VxN.z = v_reinterpret_as_f32(v_extract<2>(v_reinterpret_as_u32(VxNv), v_setzero_u32())).get0();
+
+                float dotp = -v_reduce_sum(oldN * diff);
+
+                // build point-wise upper-triangle matrix [ab^T * ab] w/o last row
+                // which is [A^T*A | A^T*b]
+                // and gather sum
+
+                v_float32x4 vd = VxNv | v_float32x4(0, 0, 0, dotp);
+                v_float32x4 n = oldN;
+                v_float32x4 nyzx;
+                {
+                    v_uint32x4 aa = v_reinterpret_as_u32(n);
+                    v_uint32x4 yz00 = v_extract<1>(aa, v_setzero_u32());
+                    v_uint32x4 x0y0, tmp;
+                    v_zip(aa, v_setzero_u32(), x0y0, tmp);
+                    nyzx = v_reinterpret_as_f32(v_combine_low(yz00, x0y0));
+                }
+
+                v_float32x4 vutg[utBufferSize];
+                for (size_t i = 0; i < utBufferSize; i++)
+                    vutg[i] = v_load_aligned(upperTriangle + i * 4);
+
+                int p = 0;
+                v_float32x4 v;
+                // vx * vd, vx * n
+                v = v_setall_f32(VxN.x);
+                v_store_aligned(upperTriangle + p * 4, v_muladd(v, vd, vutg[p])); p++;
+                v_store_aligned(upperTriangle + p * 4, v_muladd(v, n, vutg[p])); p++;
+                // vy * vd, vy * n
+                v = v_setall_f32(VxN.y);
+                v_store_aligned(upperTriangle + p * 4, v_muladd(v, vd, vutg[p])); p++;
+                v_store_aligned(upperTriangle + p * 4, v_muladd(v, n, vutg[p])); p++;
+                // vz * vd, vz * n
+                v = v_setall_f32(VxN.z);
+                v_store_aligned(upperTriangle + p * 4, v_muladd(v, vd, vutg[p])); p++;
+                v_store_aligned(upperTriangle + p * 4, v_muladd(v, n, vutg[p])); p++;
+                // nx^2, ny^2, nz^2
+                v_store_aligned(upperTriangle + p * 4, v_muladd(n, n, vutg[p])); p++;
+                // nx*ny, ny*nz, nx*nz
+                v_store_aligned(upperTriangle + p * 4, v_muladd(n, nyzx, vutg[p])); p++;
+                // nx*d, ny*d, nz*d
+                v = v_setall_f32(dotp);
+                v_store_aligned(upperTriangle + p * 4, v_muladd(n, v, vutg[p])); p++;
+            }
+        }
+
+        ABtype sumAB = ABtype::zeros();
+        for (int i = 0; i < 6; i++)
+        {
+            for (int j = i; j < 7; j++)
+            {
+                size_t p = utPos[i * 7 + j];
+                sumAB(i, j) = upperTriangle[p];
+            }
+        }
+
+#else
+        float upperTriangle[UTSIZE];
+        for (int i = 0; i < UTSIZE; i++)
+            upperTriangle[i] = 0;
+
+        for (int y = range.start; y < range.end; y++)
+        {
+            const ptype* newPtsRow = newPts[y];
+            const ptype* newNrmRow = newNrm[y];
+
+            for (int x = 0; x < newPts.cols; x++)
+            {
+                Point3f newP = fromPtype(newPtsRow[x]);
+                Point3f newN = fromPtype(newNrmRow[x]);
+
+                Point3f oldP(nan3), oldN(nan3);
+
+                if (!(fastCheck(newP) && fastCheck(newN)))
+                    continue;
+                //transform to old coord system
+                newP = pose * newP;
+                newN = pose.rotation() * newN;
+
+                //find correspondence by projecting the point
+                Point2f oldCoords = proj(newP);
+                if (!(oldCoords.x >= 0 && oldCoords.x < oldPts.cols - 1 &&
+                    oldCoords.y >= 0 && oldCoords.y < oldPts.rows - 1))
+                    continue;
+
+                // bilinearly interpolate oldPts and oldNrm under oldCoords point
+                int xi = cvFloor(oldCoords.x), yi = cvFloor(oldCoords.y);
+                float tx = oldCoords.x - xi, ty = oldCoords.y - yi;
+
+                const ptype* prow0 = oldPts[yi + 0];
+                const ptype* prow1 = oldPts[yi + 1];
+
+                Point3f p00 = fromPtype(prow0[xi + 0]);
+                Point3f p01 = fromPtype(prow0[xi + 1]);
+                Point3f p10 = fromPtype(prow1[xi + 0]);
+                Point3f p11 = fromPtype(prow1[xi + 1]);
+
+                //do not fix missing data
+                if (!(fastCheck(p00) && fastCheck(p01) &&
+                    fastCheck(p10) && fastCheck(p11)))
+                    continue;
+
+                const ptype* nrow0 = oldNrm[yi + 0];
+                const ptype* nrow1 = oldNrm[yi + 1];
+
+                Point3f n00 = fromPtype(nrow0[xi + 0]);
+                Point3f n01 = fromPtype(nrow0[xi + 1]);
+                Point3f n10 = fromPtype(nrow1[xi + 0]);
+                Point3f n11 = fromPtype(nrow1[xi + 1]);
+
+                if (!(fastCheck(n00) && fastCheck(n01) &&
+                    fastCheck(n10) && fastCheck(n11)))
+                    continue;
+
+                Point3f p0 = p00 + tx * (p01 - p00);
+                Point3f p1 = p10 + tx * (p11 - p10);
+                oldP = p0 + ty * (p1 - p0);
+
+                Point3f n0 = n00 + tx * (n01 - n00);
+                Point3f n1 = n10 + tx * (n11 - n10);
+                oldN = n0 + ty * (n1 - n0);
+
+                if (!(fastCheck(oldP) && fastCheck(oldN)))
+                    continue;
+
+                //filter by distance
+                Point3f diff = newP - oldP;
+                if (diff.dot(diff) > sqDistanceThresh)
+                {
+                    continue;
+                }
+
+                //filter by angle
+                if (abs(newN.dot(oldN)) < minCos)
+                {
+                    continue;
+                }
+                // build point-wise vector ab = [ A | b ]
+
+                //try to optimize
+                Point3f VxN = newP.cross(oldN);
+                float ab[7] = { VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, oldN.dot(-diff) };
+                // build point-wise upper-triangle matrix [ab^T * ab] w/o last row
+                // which is [A^T*A | A^T*b]
+                // and gather sum
+                int pos = 0;
+                for (int i = 0; i < 6; i++)
+                {
+                    for (int j = i; j < 7; j++)
+                    {
+                        upperTriangle[pos++] += ab[i] * ab[j];
+                    }
+                }
+            }
+        }
+
+        ABtype sumAB = ABtype::zeros();
+        int pos = 0;
+        for (int i = 0; i < 6; i++)
+        {
+            for (int j = i; j < 7; j++)
+            {
+                sumAB(i, j) = upperTriangle[pos++];
+            }
+        }
+#endif
+        AutoLock al(mtx);
+        globalSumAb += sumAB;
+    }
+
+    ABtype& globalSumAb;
+    Mutex& mtx;
+    const Points& oldPts;
+    const Normals& oldNrm;
+    const Points& newPts;
+    const Normals& newNrm;
+    Affine3f pose;
+    const Intr::Projector proj;
+    float sqDistanceThresh;
+    float minCos;
+};
+
+void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm,
+    cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b)
+{
+    CV_Assert(oldPts.size() == oldNrm.size());
+    CV_Assert(newPts.size() == newNrm.size());
+
+    CV_OCL_RUN(ocl::isOpenCLActivated(),
+        ocl_calcICPLsmMatricesFast(cameraMatrix,
+            oldPts.getUMat(AccessFlag::ACCESS_READ), oldNrm.getUMat(AccessFlag::ACCESS_READ),
+            newPts.getUMat(AccessFlag::ACCESS_READ), newNrm.getUMat(AccessFlag::ACCESS_READ),
+            pose, level, maxDepthDiff, angleThreshold,
+            A, b)
+        );
+
+    ABtype sumAB = ABtype::zeros();
+    Mutex mutex;
+    const Points  op(oldPts), np(newPts);
+    const Normals on(oldNrm),  nn(newNrm);
+
+
+    Intr intrinsics(cameraMatrix);
+    GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose,
+        intrinsics.scale(level).makeProjector(),
+        maxDepthDiff * maxDepthDiff, std::cos(angleThreshold));
+    Range range(0, newPts.rows);
+    const int nstripes = -1;
+    parallel_for_(range, invoker, nstripes);
+
+    // splitting AB matrix to A and b
+    for (int i = 0; i < 6; i++)
+    {
+        // augment lower triangle of A by symmetry
+        for (int j = i; j < 6; j++)
+        {
+            A(i, j) = A(j, i) = sumAB(i, j);
+        }
+
+        b(i) = sumAB(i, 6);
+    }
+}
+
+#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)
+{
+    CV_TRACE_FUNCTION();
+
+    Size oldSize = oldPts.size(), newSize = newPts.size();
+    CV_Assert(oldSize == oldNrm.size());
+    CV_Assert(newSize == newNrm.size());
+
+    // calculate 1x7 vector ab to produce b and upper triangle of A:
+    // [A|b] = ab*(ab^t)
+    // and then reduce it across work groups
+
+    UMat groupedSumBuffer;
+    cv::String errorStr;
+    String name = "getAb";
+    ocl::ProgramSource source = ocl::_3d::icp_oclsrc;
+    cv::String options = "-cl-mad-enable";
+    ocl::Kernel k;
+    k.create(name.c_str(), source, options, &errorStr);
+
+    if (k.empty())
+        throw std::runtime_error("Failed to create kernel: " + errorStr);
+
+    size_t globalSize[2];
+    globalSize[0] = (size_t)newPts.cols;
+    globalSize[1] = (size_t)newPts.rows;
+
+    const ocl::Device& device = ocl::Device::getDefault();
+    size_t wgsLimit = device.maxWorkGroupSize();
+    size_t memSize = device.localMemSize();
+    // local memory should keep upperTriangles for all threads in group for reduce
+    const size_t ltsz = UTSIZE * sizeof(float);
+    const size_t lcols = 32;
+    size_t lrows = min(memSize / ltsz, wgsLimit) / lcols;
+    // round lrows down to 2^n
+    lrows = roundDownPow2(lrows);
+    size_t localSize[2] = { lcols, lrows };
+    Size ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]),
+        (int)divUp(globalSize[1], (unsigned int)localSize[1]));
+
+    // size of local buffer for group-wide reduce
+    size_t lsz = localSize[0] * localSize[1] * ltsz;
+
+    Intr intrinsics(cameraMatrix);
+    Intr::Projector proj = intrinsics.scale(level).makeProjector();
+    Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy);
+
+    UMat& groupedSumGpu = groupedSumBuffer;
+    groupedSumGpu.create(Size(ngroups.width * UTSIZE, ngroups.height),
+        CV_32F);
+    groupedSumGpu.setTo(0);
+
+    // TODO: optimization possible:
+    // samplers instead of oldPts/oldNrm (mask needed)
+    k.args(ocl::KernelArg::ReadOnlyNoSize(oldPts),
+        ocl::KernelArg::ReadOnlyNoSize(oldNrm),
+        oldSize,
+        ocl::KernelArg::ReadOnlyNoSize(newPts),
+        ocl::KernelArg::ReadOnlyNoSize(newNrm),
+        newSize,
+        ocl::KernelArg::Constant(pose.matrix.val,
+            sizeof(pose.matrix.val)),
+        fxy.val, cxy.val,
+        maxDepthDiff * maxDepthDiff,
+        std::cos(angleThreshold),
+        ocl::KernelArg::Local(lsz),
+        ocl::KernelArg::WriteOnlyNoSize(groupedSumGpu)
+    );
+
+    if (!k.run(2, globalSize, localSize, true))
+        throw std::runtime_error("Failed to run kernel");
+
+    float upperTriangle[UTSIZE];
+    for (int i = 0; i < UTSIZE; i++)
+        upperTriangle[i] = 0;
+
+    Mat groupedSumCpu = groupedSumGpu.getMat(ACCESS_READ);
+
+    for (int y = 0; y < ngroups.height; y++)
+    {
+        const float* rowr = groupedSumCpu.ptr<float>(y);
+        for (int x = 0; x < ngroups.width; x++)
+        {
+            const float* p = rowr + x * UTSIZE;
+            for (int j = 0; j < UTSIZE; j++)
+            {
+                upperTriangle[j] += p[j];
+            }
+        }
+    }
+    groupedSumCpu.release();
+
+    ABtype sumAB = ABtype::zeros();
+    int pos = 0;
+    for (int i = 0; i < 6; i++)
+    {
+        for (int j = i; j < 7; j++)
+        {
+            sumAB(i, j) = upperTriangle[pos++];
+        }
+    }
+
+    // splitting AB matrix to A and b
+    for (int i = 0; i < 6; i++)
+    {
+        // augment lower triangle of A by symmetry
+        for (int j = i; j < 6; j++)
+        {
+            A(i, j) = A(j, i) = sumAB(i, j);
+        }
+
+        b(i) = sumAB(i, 6);
+    }
+    return true;
+}
+
+#endif
+
+}
diff --git a/modules/3d/src/rgbd/odometry_functions.hpp b/modules/3d/src/rgbd/odometry_functions.hpp
new file mode 100644
index 0000000000..763b143cb4
--- /dev/null
+++ b/modules/3d/src/rgbd/odometry_functions.hpp
@@ -0,0 +1,212 @@
+// 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 = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4
+};
+
+static inline
+void checkImage(InputArray image)
+{
+    if (image.empty())
+        CV_Error(Error::StsBadSize, "Image is empty.");
+    if (image.type() != CV_8UC1)
+        CV_Error(Error::StsBadSize, "Image type has to be CV_8UC1.");
+}
+static inline
+void checkDepth(InputArray depth, const Size& imageSize)
+{
+    if (depth.empty())
+        CV_Error(Error::StsBadSize, "Depth is empty.");
+    if (depth.size() != imageSize)
+        CV_Error(Error::StsBadSize, "Depth has to have the size equal to the image size.");
+    if (depth.type() != CV_32FC1)
+        CV_Error(Error::StsBadSize, "Depth type has to be CV_32FC1.");
+}
+
+static inline
+void checkMask(InputArray mask, const Size& imageSize)
+{
+    if (!mask.empty())
+    {
+        if (mask.size() != imageSize)
+            CV_Error(Error::StsBadSize, "Mask has to have the size equal to the image size.");
+        if (mask.type() != CV_8UC1)
+            CV_Error(Error::StsBadSize, "Mask type has to be CV_8UC1.");
+    }
+}
+
+static inline
+void checkNormals(InputArray normals, const Size& depthSize)
+{
+    if (normals.size() != depthSize)
+        CV_Error(Error::StsBadSize, "Normals has to have the size equal to the depth size.");
+    if (normals.type() != CV_32FC3)
+        CV_Error(Error::StsBadSize, "Normals type has to be CV_32FC3.");
+}
+
+
+static inline
+void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& 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;
+
+    C[0] = -p3d.z * v1 + p3d.y * v2;
+    C[1] = p3d.z * v0 - p3d.x * v2;
+    C[2] = -p3d.y * v0 + p3d.x * v1;
+    C[3] = v0;
+    C[4] = v1;
+    C[5] = v2;
+}
+
+static inline
+void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const Point3f& 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;
+    C[0] = -p3d.z * v1 + p3d.y * v2;
+    C[1] = p3d.z * v0 - p3d.x * v2;
+    C[2] = -p3d.y * v0 + p3d.x * v1;
+}
+
+static inline
+void calcRgbdEquationCoeffsTranslation(double* C, double dIdx, double dIdy, const Point3f& 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;
+    C[0] = v0;
+    C[1] = v1;
+    C[2] = v2;
+}
+
+typedef
+void (*CalcRgbdEquationCoeffsPtr)(double*, double, double, const Point3f&, double, double);
+
+static inline
+void calcICPEquationCoeffs(double* C, const Point3f& p0, const Vec3f& n1)
+{
+    C[0] = -p0.z * n1[1] + p0.y * n1[2];
+    C[1] = p0.z * n1[0] - p0.x * n1[2];
+    C[2] = -p0.y * n1[0] + p0.x * n1[1];
+    C[3] = n1[0];
+    C[4] = n1[1];
+    C[5] = n1[2];
+}
+
+static inline
+void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Vec3f& n1)
+{
+    C[0] = -p0.z * n1[1] + p0.y * n1[2];
+    C[1] = p0.z * n1[0] - p0.x * n1[2];
+    C[2] = -p0.y * n1[0] + p0.x * n1[1];
+}
+
+static inline
+void calcICPEquationCoeffsTranslation(double* C, const Point3f& /*p0*/, const Vec3f& n1)
+{
+    C[0] = n1[0];
+    C[1] = n1[1];
+    C[2] = n1[2];
+}
+
+typedef
+void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&);
+
+void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
+void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings);
+void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype);
+
+void prepareRGBFrameBase(OdometryFrame& frame, const OdometrySettings settings);
+void prepareRGBFrameSrc (OdometryFrame& frame, const OdometrySettings settings);
+void prepareRGBFrameDst (OdometryFrame& frame, const OdometrySettings settings);
+
+void prepareICPFrameBase(OdometryFrame& frame, const OdometrySettings settings);
+void prepareICPFrameSrc (OdometryFrame& frame, const OdometrySettings settings);
+void prepareICPFrameDst (OdometryFrame& frame, const OdometrySettings settings);
+
+
+void setPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, InputArrayOfArrays pyramidImage);
+void getPyramids(OdometryFrame& odf, OdometryFramePyramidType oftype, OutputArrayOfArrays _pyramid);
+
+void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage, size_t levelCount);
+
+template<typename TMat>
+void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth, InputArrayOfArrays pyramidNormal, InputOutputArrayOfArrays pyramidMask);
+
+template<typename TMat>
+void preparePyramidCloud(InputArrayOfArrays pyramidDepth, const Matx33f& cameraMatrix, InputOutputArrayOfArrays pyramidCloud);
+
+void buildPyramidCameraMatrix(const Matx33f& cameraMatrix, int levels, std::vector<Matx33f>& pyramidCameraMatrix);
+
+template<typename TMat>
+void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel, int sobelSize);
+
+void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy,
+    InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart,
+    InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale);
+
+void randomSubsetOfMask(InputOutputArray _mask, float part);
+
+void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals);
+
+void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart,
+    InputOutputArrayOfArrays /*std::vector<Mat>&*/ pyramidNormalsMask);
+
+
+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 Matx33f& _K_inv, 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, CalcRgbdEquationCoeffsPtr func, int transformDim);
+
+void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt,
+    const Mat& cloud1, const Mat& normals1,
+    const Mat& corresps,
+    Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim);
+
+void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& 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
diff --git a/modules/3d/src/rgbd/odometry_settings_impl.cpp b/modules/3d/src/rgbd/odometry_settings_impl.cpp
new file mode 100644
index 0000000000..efc780f9c1
--- /dev/null
+++ b/modules/3d/src/rgbd/odometry_settings_impl.cpp
@@ -0,0 +1,354 @@
+// 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  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  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 angleThreshold;
+    float maxTranslation;
+    float maxRotation;
+
+    float minGradientMagnitude;
+    std::vector<float> minGradientMagnitudes;
+
+public:
+    class DefaultSets {
+    public:
+        const cv::Matx33f defaultCameraMatrix =
+        { /* fx, 0, cx*/ 0, 0, 0,
+            /* 0, fy, cy*/ 0, 0, 0,
+            /* 0,  0,  0*/ 0, 0, 0 };
+        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 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>();
+}
+
+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::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->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::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);
+}
+
+}
diff --git a/modules/3d/src/rgbd/plane.cpp b/modules/3d/src/rgbd/plane.cpp
index 3a580397ee..6719133b7f 100644
--- a/modules/3d/src/rgbd/plane.cpp
+++ b/modules/3d/src/rgbd/plane.cpp
@@ -178,7 +178,7 @@ private:
 class PlaneGrid
 {
 public:
-    PlaneGrid(const Mat_<Vec3f>& points3d, int block_size) :
+    PlaneGrid(const Mat_<Vec4f>& points3d, int block_size) :
         block_size_(block_size)
     {
         // Figure out some dimensions
@@ -204,10 +204,10 @@ public:
                 int K = 0;
                 for (int j = y * block_size; j < std::min((y + 1) * block_size, points3d.rows); ++j)
                 {
-                    const Vec3f* vec = points3d.ptr < Vec3f >(j, x * block_size), * vec_end;
+                    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 < Vec3f >(j, points3d.cols - 1) + 1;
+                        vec_end = points3d.ptr < Vec4f >(j, points3d.cols - 1) + 1;
                     else
                         vec_end = vec + block_size;
                     for (; vec != vec_end; ++vec, pointpointt += 9)
@@ -226,7 +226,7 @@ public:
                         *(pointpointt + 8) = vec->val[2] * vec->val[2];
 
                         Q += *reinterpret_cast<Matx33f*>(pointpointt);
-                        m += (*vec);
+                        m += Vec3f((*vec)[0], (*vec)[1], (*vec)[2]);
                         ++K;
                     }
                 }
@@ -327,7 +327,7 @@ private:
 class InlierFinder
 {
 public:
-    InlierFinder(float err, const Mat_<Vec3f>& points3d, const Mat_<Vec3f>& normals,
+    InlierFinder(float err, const Mat_<Vec4f>& points3d, const Mat_<Vec4f>& normals,
                  unsigned char plane_index, int block_size) :
         err_(err),
         points3d_(points3d),
@@ -362,14 +362,14 @@ public:
         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 Vec3f* point = points3d_.ptr < Vec3f >(yy, range_x.start);
+            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 Vec3f* normal = normals_.ptr < Vec3f >(yy, range_x.start);
+                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
@@ -377,13 +377,15 @@ public:
                         continue;
 
                     // If the point is close enough to the plane
-                    if (plane->distance(*point) < err_)
+                    Vec3f _p = Vec3f((*point)[0], (*point)[1], (*point)[2]);
+                    if (plane->distance(_p) < err_)
                     {
                         // make sure the normals are similar to the plane
-                        if (std::abs(plane->n().dot(*normal)) > 0.3)
+                        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(*point, *Q_local);
+                            plane->UpdateStatistics(_p, *Q_local);
                             *data = plane_index_;
                             ++n_valid_points;
                         }
@@ -399,10 +401,11 @@ public:
                         continue;
 
                     // If the point is close enough to the plane
-                    if (plane->distance(*point) < err_)
+                    Vec3f _p = Vec3f((*point)[0], (*point)[1], (*point)[2]);
+                    if (plane->distance(_p) < err_)
                     {
                         // The point now belongs to the plane
-                        plane->UpdateStatistics(*point, *Q_local);
+                        plane->UpdateStatistics(_p, *Q_local);
                         *data = plane_index_;
                         ++n_valid_points;
                     }
@@ -461,8 +464,8 @@ public:
 
 private:
     float err_;
-    const Mat_<Vec3f>& points3d_;
-    const Mat_<Vec3f>& normals_;
+    const Mat_<Vec4f>& points3d_;
+    const Mat_<Vec4f>& normals_;
     unsigned char plane_index_;
     /** THe block size as defined in the main algorithm */
     int block_size_;
@@ -478,7 +481,7 @@ void findPlanes(InputArray points3d_in, InputArray normals_in, OutputArray mask_
 {
     CV_Assert(method == RGBD_PLANE_METHOD_DEFAULT);
 
-    Mat_<Vec3f> points3d, normals;
+    Mat_<Vec4f> points3d, normals;
     if (points3d_in.depth() == CV_32F)
         points3d = points3d_in.getMat();
     else
diff --git a/modules/3d/src/rgbd/utils.cpp b/modules/3d/src/rgbd/utils.cpp
index 985c14115e..6227895dbc 100644
--- a/modules/3d/src/rgbd/utils.cpp
+++ b/modules/3d/src/rgbd/utils.cpp
@@ -8,6 +8,36 @@
 namespace cv
 {
 
+template<typename TMat>
+inline TMat getTMat(InputArray, int)
+{
+    return TMat();
+}
+
+template<>
+Mat getTMat<Mat>(InputArray a, int i)
+{
+    return a.getMat(i);
+}
+
+template<>
+UMat getTMat<UMat>(InputArray a, int i)
+{
+    return a.getUMat(i);
+}
+
+template<typename TMat>
+inline TMat& getTMatRef(InputOutputArray, int)
+{
+    return TMat();
+}
+
+template<>
+Mat& getTMatRef<Mat>(InputOutputArray a, int i)
+{
+    return a.getMatRef(i);
+}
+
 /** 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
diff --git a/modules/3d/src/rgbd/utils.hpp b/modules/3d/src/rgbd/utils.hpp
index 056b207899..d0fcf1c748 100644
--- a/modules/3d/src/rgbd/utils.hpp
+++ b/modules/3d/src/rgbd/utils.hpp
@@ -105,6 +105,18 @@ static inline bool isNaN(const cv::v_float32x4& p)
 }
 #endif
 
+template<typename TMat>
+inline TMat getTMat(InputArray, int = -1);
+template<>
+Mat getTMat<Mat>(InputArray a, int i);
+template<>
+UMat getTMat<UMat>(InputArray a, int i);
+
+template<typename TMat>
+inline TMat& getTMatRef(InputOutputArray, int = -1);
+template<>
+Mat& getTMatRef<Mat>(InputOutputArray a, int i);
+
 inline size_t roundDownPow2(size_t x)
 {
     size_t shift = 0;
@@ -183,6 +195,18 @@ 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,
diff --git a/modules/3d/test/test_normal.cpp b/modules/3d/test/test_normal.cpp
index b4ce20d2f2..af176a5a42 100644
--- a/modules/3d/test/test_normal.cpp
+++ b/modules/3d/test/test_normal.cpp
@@ -17,12 +17,12 @@ rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const M
 }
 #endif
 
-Vec3f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec3d& normal, const Matx33d& Kinv)
+Vec4f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec4d& normal, const Matx33d& Kinv)
 {
     Matx31d L = Kinv * uv1; //a ray passing through camera optical center
     //and uv.
     L = L * (1.0 / cv::norm(L));
-    double LdotNormal = L.dot(normal);
+    double LdotNormal = L.dot(Vec3d(normal[0], normal[1], normal[2]));
     double d;
     if (std::fabs(LdotNormal) > 1e-9)
     {
@@ -34,7 +34,7 @@ Vec3f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const V
         std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl;
         std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl;
     }
-    Vec3f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)));
+    Vec4f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)), 0);
     return xyz;
 }
 
@@ -48,9 +48,9 @@ float cy = H / 2.f + 0.5f;
 Mat K = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1);
 Mat Kinv = K.inv();
 
-void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap);
+void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap);
 
-void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap)
+void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap)
 {
     std::vector<Point3f> points3dvec;
     for (int i = 0; i < H; i++)
@@ -80,13 +80,14 @@ void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap)
 static RNG rng;
 struct Plane
 {
-    Vec3d n, p;
+    Vec4d n, p;
     double p_dot_n;
     Plane()
     {
         n[0] = rng.uniform(-0.5, 0.5);
         n[1] = rng.uniform(-0.5, 0.5);
         n[2] = -0.3; //rng.uniform(-1.f, 0.5f);
+        n[3] = 0.;
         n = n / cv::norm(n);
         set_d((float)rng.uniform(-2.0, 0.6));
     }
@@ -94,11 +95,11 @@ struct Plane
     void
         set_d(float d)
     {
-        p = Vec3d(0, 0, d / n[2]);
+        p = Vec4d(0, 0, d / n[2], 0);
         p_dot_n = p.dot(n);
     }
 
-    Vec3f
+    Vec4f
         intersection(float u, float v, const Matx33f& Kinv_in) const
     {
         return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
@@ -118,8 +119,8 @@ void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_ma
             planes.push_back(px);
         }
     }
-    Mat_ < Vec3f > outp(H, W);
-    Mat_ < Vec3f > outn(H, W);
+    Mat_ < Vec4f > outp(H, W);
+    Mat_ < Vec4f > outn(H, W);
     plane_mask.create(H, W);
 
     // n  ( r - r_0) = 0
@@ -282,15 +283,15 @@ public:
             normals_computer->apply(points3d, in_normals);
         tm.stop();
 
-        Mat_<Vec3f> normals, ground_normals;
-        in_normals.convertTo(normals, CV_32FC3);
-        in_ground_normals.convertTo(ground_normals, CV_32FC3);
+        Mat_<Vec4f> normals, ground_normals;
+        in_normals.convertTo(normals, CV_32FC4);
+        in_ground_normals.convertTo(ground_normals, CV_32FC4);
 
         float err = 0;
         for (int y = 0; y < normals.rows; ++y)
             for (int x = 0; x < normals.cols; ++x)
             {
-                Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x);
+                Vec4f vec1 = normals(y, x), vec2 = ground_normals(y, x);
                 vec1 = vec1 / cv::norm(vec1);
                 vec2 = vec2 / cv::norm(vec2);
 
@@ -381,7 +382,8 @@ public:
                 ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001);
                 // Compare the normals
                 Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]);
-                ASSERT_GE(std::abs(gt_planes[j].n.dot(normal)), 0.95);
+                Vec4d n = gt_planes[j].n;
+                ASSERT_GE(std::abs(Vec3d(n[0], n[1], n[2]).dot(normal)), 0.95);
             }
 
             CV_LOG_INFO(NULL, "Speed: ");
diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp
index 71569f5304..395b6fe7fb 100644
--- a/modules/3d/test/test_odometry.cpp
+++ b/modules/3d/test/test_odometry.cpp
@@ -6,8 +6,6 @@
 
 namespace opencv_test { namespace {
 
-#define SHOW_DEBUG_IMAGES  0
-
 static
 void warpFrame(const Mat& image, const Mat& depth, const Mat& rvec, const Mat& tvec, const Mat& K,
                Mat& warpedImage, Mat& warpedDepth)
@@ -33,6 +31,12 @@ void warpFrame(const Mat& image, const Mat& depth, const Mat& rvec, const Mat& t
 
     Mat cloud;
     depthTo3d(depth, K, cloud);
+
+    Mat cloud3, channels[4];
+    cv::split(cloud, channels);
+    std::vector<Mat> merged = { channels[0], channels[1], channels[2] };
+    cv::merge(merged, cloud3);
+
     Mat Rt = Mat::eye(4, 4, CV_64FC1);
     {
         Mat R, dst;
@@ -45,7 +49,7 @@ void warpFrame(const Mat& image, const Mat& depth, const Mat& rvec, const Mat& t
         tvec.copyTo(dst);
     }
     Mat warpedCloud, warpedImagePoints;
-    perspectiveTransform(cloud, warpedCloud, Rt);
+    perspectiveTransform(cloud3, warpedCloud, Rt);
     projectPoints(warpedCloud.reshape(3, 1), Mat(3,1,CV_32FC1, Scalar(0)), Mat(3,1,CV_32FC1, Scalar(0)), K, Mat(1,5,CV_32FC1, Scalar(0)), warpedImagePoints);
     warpedImagePoints = warpedImagePoints.reshape(2, cloud.rows);
     Rect r(0, 0, image.cols, image.rows);
@@ -116,11 +120,13 @@ void dilateFrame(Mat& image, Mat& depth)
 class OdometryTest
 {
 public:
-    OdometryTest(const Ptr<Odometry>& _odometry,
+    OdometryTest(OdometryType _otype,
+                 OdometryAlgoType _algtype,
                  double _maxError1,
                  double _maxError5,
                  double _idError = DBL_EPSILON) :
-        odometry(_odometry),
+        otype(_otype),
+        algtype(_algtype),
         maxError1(_maxError1),
         maxError5(_maxError5),
         idError(_idError)
@@ -143,7 +149,8 @@ public:
     void run();
     void checkUMats();
 
-    Ptr<Odometry> odometry;
+    OdometryType otype;
+    OdometryAlgoType algtype;
     double maxError1;
     double maxError5;
     double idError;
@@ -201,24 +208,30 @@ void OdometryTest::checkUMats()
     Mat image, depth;
     readData(image, depth);
 
-    odometry->setCameraMatrix(K);
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf = odometry.createOdometryFrame(OdometryFrameStoreType::UMAT);
 
     Mat calcRt;
 
-    UMat uimage, udepth, umask;
+    UMat uimage, udepth;
     image.copyTo(uimage);
     depth.copyTo(udepth);
-    Mat(image.size(), CV_8UC1, Scalar(255)).copyTo(umask);
+    odf.setImage(uimage);
+    odf.setDepth(udepth);
+    uimage.release();
+    udepth.release();
 
-    bool isComputed = odometry->compute(uimage, udepth, umask,
-                                        uimage, udepth, umask,
-                                        calcRt);
+    odometry.prepareFrame(odf);
+    bool isComputed = odometry.compute(odf, odf, calcRt);
     ASSERT_TRUE(isComputed);
     double diff = cv::norm(calcRt, Mat::eye(4, 4, CV_64FC1));
     if (diff > idError)
     {
         FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << diff << std::endl;
     }
+
 }
 
 void OdometryTest::run()
@@ -227,22 +240,28 @@ void OdometryTest::run()
 
     Mat image, depth;
     readData(image, depth);
-
-    odometry->setCameraMatrix(K);
-
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf = odometry.createOdometryFrame();
+    odf.setImage(image);
+    odf.setDepth(depth);
     Mat calcRt;
 
     // 1. Try to find Rt between the same frame (try masks also).
     Mat mask(image.size(), CV_8UC1, Scalar(255));
-    bool isComputed = odometry->compute(image, depth, mask, image, depth, mask, calcRt);
+
+    odometry.prepareFrame(odf);
+    bool isComputed = odometry.compute(odf, odf, calcRt);
+
     if(!isComputed)
     {
         FAIL() << "Can not find Rt between the same frame" << std::endl;
     }
-    double diff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1));
-    if(diff > idError)
+    double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1));
+    if(ndiff > idError)
     {
-        FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << diff << std::endl;
+        FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl;
     }
 
     // 2. Generate random rigid body motion in some ranges several times (iterCount).
@@ -254,45 +273,62 @@ void OdometryTest::run()
     int iterCount = 100;
     int better_1time_count = 0;
     int better_5times_count = 0;
-    for(int iter = 0; iter < iterCount; iter++)
+    for (int iter = 0; iter < iterCount; iter++)
     {
         Mat rvec, tvec;
         generateRandomTransformation(rvec, tvec);
+
         Mat warpedImage, warpedDepth;
         warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth);
         dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
 
-        isComputed = odometry->compute(image, depth, mask, warpedImage, warpedDepth, mask, calcRt);
-        if(!isComputed)
-            continue;
+        OdometryFrame odfSrc = odometry.createOdometryFrame();
+        OdometryFrame odfDst = odometry.createOdometryFrame();
+        odfSrc.setImage(image);
+        odfSrc.setDepth(depth);
+        odfDst.setImage(warpedImage);
+        odfDst.setDepth(warpedDepth);
 
+        odometry.prepareFrames(odfSrc, odfDst);
+        isComputed = odometry.compute(odfSrc, odfDst, calcRt);
+
+        if (!isComputed)
+            continue;
         Mat calcR = calcRt(Rect(0,0,3,3)), calcRvec;
         cv::Rodrigues(calcR, calcRvec);
         calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows);
         Mat calcTvec = calcRt(Rect(3,0,1,3));
 
-#if SHOW_DEBUG_IMAGES
-        imshow("image", image);
-        imshow("warpedImage", warpedImage);
-        Mat resultImage, resultDepth;
-        warpFrame(image, depth, calcRvec, calcTvec, K, resultImage, resultDepth);
-        imshow("resultImage", resultImage);
-        waitKey();
-#endif
+        if (cvtest::debugLevel >= 10)
+        {
+            imshow("image", image);
+            imshow("warpedImage", warpedImage);
+            Mat resultImage, resultDepth;
+            warpFrame(image, depth, calcRvec, calcTvec, K, resultImage, resultDepth);
+            imshow("resultImage", resultImage);
+            waitKey(100);
+        }
 
         // compare rotation
-        double rdiffnorm = cv::norm(rvec - calcRvec),
-               rnorm = cv::norm(rvec);
-        double tdiffnorm = cv::norm(tvec - calcTvec),
-               tnorm = cv::norm(tvec);
-        if(rdiffnorm < rnorm &&  tdiffnorm < tnorm)
+        double possibleError = algtype == OdometryAlgoType::COMMON ? 0.09f : 0.015f;
+
+        Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec));
+        Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec));
+        Affine3f src_inv = src.inv();
+        Affine3f diff = res * src_inv;
+        double rdiffnorm = cv::norm(diff.rvec());
+        double tdiffnorm = cv::norm(diff.translation());
+
+        if (rdiffnorm < possibleError && tdiffnorm < possibleError)
+        {
             better_1time_count++;
-        if(5. * rdiffnorm < rnorm && 5 * tdiffnorm < tnorm)
+        }
+        if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError)
             better_5times_count++;
 
         CV_LOG_INFO(NULL, "Iter " << iter);
-        CV_LOG_INFO(NULL, "rdiffnorm " << rdiffnorm << "; rnorm " << rnorm);
-        CV_LOG_INFO(NULL, "tdiffnorm " << tdiffnorm << "; tnorm " << tnorm);
+        CV_LOG_INFO(NULL, "rdiff: " << Vec3f(diff.rvec()) << "; rdiffnorm: " << rdiffnorm);
+        CV_LOG_INFO(NULL, "tdiff: " << Vec3f(diff.translation()) << "; tdiffnorm: " << tdiffnorm);
 
         CV_LOG_INFO(NULL, "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count);
     }
@@ -318,89 +354,51 @@ void OdometryTest::run()
 
 TEST(RGBD_Odometry_Rgbd, algorithmic)
 {
-    OdometryTest test(cv::Odometry::createFromName("RgbdOdometry"), 0.99, 0.89);
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.89);
     test.run();
 }
 
 TEST(RGBD_Odometry_ICP, algorithmic)
 {
-    OdometryTest test(cv::Odometry::createFromName("ICPOdometry"), 0.99, 0.99);
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
     test.run();
 }
 
 TEST(RGBD_Odometry_RgbdICP, algorithmic)
 {
-    OdometryTest test(cv::Odometry::createFromName("RgbdICPOdometry"), 0.99, 0.99);
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
     test.run();
 }
 
 TEST(RGBD_Odometry_FastICP, algorithmic)
 {
-    OdometryTest test(cv::Odometry::createFromName("FastICPOdometry"), 0.99, 0.99, FLT_EPSILON);
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
     test.run();
 }
 
+
 TEST(RGBD_Odometry_Rgbd, UMats)
 {
-    OdometryTest test(cv::Odometry::createFromName("RgbdOdometry"), 0.99, 0.89);
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.89);
     test.checkUMats();
 }
 
 TEST(RGBD_Odometry_ICP, UMats)
 {
-    OdometryTest test(cv::Odometry::createFromName("ICPOdometry"), 0.99, 0.99);
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
     test.checkUMats();
 }
 
 TEST(RGBD_Odometry_RgbdICP, UMats)
 {
-    OdometryTest test(cv::Odometry::createFromName("RgbdICPOdometry"), 0.99, 0.99);
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
     test.checkUMats();
 }
 
 TEST(RGBD_Odometry_FastICP, UMats)
 {
-    OdometryTest test(cv::Odometry::createFromName("FastICPOdometry"), 0.99, 0.99, FLT_EPSILON);
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
     test.checkUMats();
 }
 
-
-/****************************************************************************************\
-*                                Depth to 3d tests                                       *
-\****************************************************************************************/
-
-TEST(RGBD_DepthTo3d, compute)
-{
-    // K from a VGA Kinect
-    Mat K = OdometryTest::getCameraMatrix();
-
-    // Create a random depth image
-    RNG rng;
-    Mat_<float> depth(480, 640);
-    rng.fill(depth, RNG::UNIFORM, 0, 100);
-
-    // Create some 3d points on the plane
-    int rows = depth.rows, cols = depth.cols;
-    Mat_<Vec3f> points3d;
-    depthTo3d(depth, K, points3d);
-
-    // Make sure the points belong to the plane
-    Mat points = points3d.reshape(1, rows * cols);
-    Mat image_points;
-    Mat rvec;
-    cv::Rodrigues(Mat::eye(3, 3, CV_32F), rvec);
-    Mat tvec = (Mat_<float>(1, 3) << 0, 0, 0);
-    projectPoints(points, rvec, tvec, K, Mat(), image_points);
-    image_points = image_points.reshape(2, rows);
-
-    float avg_diff = 0;
-    for (int y = 0; y < rows; ++y)
-        for (int x = 0; x < cols; ++x)
-            avg_diff += (float)cv::norm(image_points.at<Vec2f>(y, x) - Vec2f((float)x, (float)y));
-
-    // Verify the function works
-    ASSERT_LE(avg_diff / rows / cols, 1e-4) << "Average error for ground truth is: " << (avg_diff / rows / cols);
-}
-
-
 }} // namespace