Merge pull request #10 from vrabaud/master

add an RGBD module

modules/rgbd/CMakeLists.txt

+set(the_description "RGBD algorithms")
+ocv_warnings_disable(CMAKE_CXX_FLAGS -Wundef)
+ocv_define_module(rgbd opencv_core opencv_calib3d opencv_contrib opencv_highgui opencv_imgproc)

modules/rgbd/doc/rgbd.rst

+**************************
+rgbd. RGB-Depth Processing
+**************************
+
+.. highlight:: cpp
+
+.. toctree::
+    :maxdepth: 2
+
+    depth_image
+    geometry
+    odometry

modules/rgbd/include/opencv2/rgbd.hpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __OPENCV_RGBD_HPP__
+#define __OPENCV_RGBD_HPP__
+
+#ifdef __cplusplus
+
+#include <limits>
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+  /** Checks if the value is a valid depth. For CV_16U or CV_16S, the convention is to be invalid if it is
+   * a limit. For a float/double, we just check if it is a NaN
+   * @param depth the depth to check for validity
+   */
+  CV_EXPORTS
+  inline bool
+  isValidDepth(const float & depth)
+  {
+    return !cvIsNaN(depth);
+  }
+  CV_EXPORTS
+  inline bool
+  isValidDepth(const double & depth)
+  {
+    return !cvIsNaN(depth);
+  }
+  CV_EXPORTS
+  inline bool
+  isValidDepth(const short int & depth)
+  {
+    return (depth != std::numeric_limits<short int>::min()) && (depth != std::numeric_limits<short int>::max());
+  }
+  CV_EXPORTS
+  inline bool
+  isValidDepth(const unsigned short int & depth)
+  {
+    return (depth != std::numeric_limits<unsigned short int>::min())
+        && (depth != std::numeric_limits<unsigned short int>::max());
+  }
+  CV_EXPORTS
+  inline bool
+  isValidDepth(const int & depth)
+  {
+    return (depth != std::numeric_limits<int>::min()) && (depth != std::numeric_limits<int>::max());
+  }
+  CV_EXPORTS
+  inline bool
+  isValidDepth(const unsigned int & depth)
+  {
+    return (depth != std::numeric_limits<unsigned int>::min()) && (depth != std::numeric_limits<unsigned int>::max());
+  }
+
+  /** Object that can compute the normals in an image.
+   * It is an object as it can cache data for speed efficiency
+   * The implemented methods are either:
+   * - FALS (the fastest) and SRI from
+   * ``Fast and Accurate Computation of Surface Normals from Range Images``
+   * by H. Badino, D. Huber, Y. Park and T. Kanade
+   * - the normals with bilateral filtering on a depth image from
+   * ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
+   * by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
+   */
+  class CV_EXPORTS RgbdNormals: public Algorithm
+  {
+  public:
+    enum RGBD_NORMALS_METHOD
+    {
+      RGBD_NORMALS_METHOD_FALS, RGBD_NORMALS_METHOD_LINEMOD, RGBD_NORMALS_METHOD_SRI
+    };
+
+    RgbdNormals()
+        :
+          rows_(0),
+          cols_(0),
+          depth_(0),
+          K_(Mat()),
+          window_size_(0),
+          method_(RGBD_NORMALS_METHOD_FALS),
+          rgbd_normals_impl_(0)
+    {
+    }
+
+    /** Constructor
+     * @param rows the number of rows of the depth image normals will be computed on
+     * @param cols the number of cols of the depth image normals will be computed on
+     * @param depth the depth of the normals (only CV_32F or CV_64F for FALS and SRI, CV_16U for LINEMOD)
+     * @param K the calibration matrix to use
+     * @param window_size the window size to compute the normals: can only be 1,3,5 or 7
+     * @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS
+     */
+    RgbdNormals(int rows, int cols, int depth, InputArray K, int window_size = 5, int method =
+        RGBD_NORMALS_METHOD_FALS);
+
+    ~RgbdNormals();
+
+    AlgorithmInfo*
+    info() const;
+
+    /** Given a set of 3d points in a depth image, compute the normals at each point.
+     * @param points a rows x cols x 3 matrix of CV_32F/CV64F or a rows x cols x 1 CV_U16S
+     * @param normals a rows x cols x 3 matrix
+     */
+    void
+    operator()(InputArray points, OutputArray normals) const;
+
+    /** Initializes some data that is cached for later computation
+     * If that function is not called, it will be called the first time normals are computed
+     */
+    void
+    initialize() const;
+
+    /** Return the current method in that normal computer
+     * @return
+     */
+    int
+    method() const
+    {
+      return method_;
+    }
+  protected:
+    void
+    initialize_normals_impl(int rows, int cols, int depth, const Mat & K, int window_size, int method) const;
+
+    int rows_, cols_, depth_;
+    Mat K_;
+    int window_size_;
+    int method_;
+    mutable void* rgbd_normals_impl_;
+  };
+
+  /** Object that can clean a noisy depth image
+   */
+  class CV_EXPORTS DepthCleaner: public Algorithm
+  {
+  public:
+    /** NIL method is from
+     * ``Modeling Kinect Sensor Noise for Improved 3d Reconstruction and Tracking``
+     * by C. Nguyen, S. Izadi, D. Lovel
+     */
+    enum DEPTH_CLEANER_METHOD
+    {
+      DEPTH_CLEANER_NIL
+    };
+
+    DepthCleaner()
+        :
+          depth_(0),
+          window_size_(0),
+          method_(DEPTH_CLEANER_NIL),
+          depth_cleaner_impl_(0)
+    {
+    }
+
+    /** Constructor
+     * @param depth the depth of the normals (only CV_32F or CV_64F for FALS and SRI, CV_16U for LINEMOD)
+     * @param window_size the window size to compute the normals: can only be 1,3,5 or 7
+     * @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS
+     */
+    DepthCleaner(int depth, int window_size = 5, int method = DEPTH_CLEANER_NIL);
+
+    ~DepthCleaner();
+
+    AlgorithmInfo*
+    info() const;
+
+    /** Given a set of 3d points in a depth image, compute the normals at each point.
+     * @param points a rows x cols x 3 matrix of CV_32F/CV64F or a rows x cols x 1 CV_U16S
+     * @param depth a rows x cols matrix of the cleaned up depth
+     */
+    void
+    operator()(InputArray points, OutputArray depth) const;
+
+    /** Initializes some data that is cached for later computation
+     * If that function is not called, it will be called the first time normals are computed
+     */
+    void
+    initialize() const;
+
+    /** Return the current method in that normal computer
+     * @return
+     */
+    int
+    method() const
+    {
+      return method_;
+    }
+  protected:
+    void
+    initialize_cleaner_impl() const;
+
+    int depth_;
+    int window_size_;
+    int method_;
+    mutable void* depth_cleaner_impl_;
+  };
+
+  /**
+   * @param depth the depth image
+   * @param K
+   * @param in_points the list of xy coordinates
+   * @param points3d the resulting 3d points
+   */
+  CV_EXPORTS
+  void
+  depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d);
+
+  /** Converts a depth image to an organized set of 3d points.
+   * The coordinate system is x pointing left, y down and z away from the camera
+   * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+   *              (as done with the Microsoft Kinect), otherwise, if given as CV_32F or CV_64F, it is assumed in meters)
+   * @param K The calibration matrix
+   * @param points3d the resulting 3d points. 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)
+   */
+  CV_EXPORTS
+  void
+  depthTo3d(InputArray depth, InputArray K, OutputArray points3d, InputArray mask = noArray());
+
+  /** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+   * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+   * Otherwise, the image is simply converted to floats
+   * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+   *              (as done with the Microsoft Kinect), it is assumed in meters)
+   * @param the desired output depth (floats or double)
+   * @param out The rescaled float depth image
+   */
+  CV_EXPORTS
+  void
+  rescaleDepth(InputArray in, int depth, OutputArray out);
+
+  /** Object that can compute planes in an image
+   */
+  class CV_EXPORTS RgbdPlane: public Algorithm
+  {
+  public:
+    enum RGBD_PLANE_METHOD
+    {
+      RGBD_PLANE_METHOD_DEFAULT
+    };
+
+    RgbdPlane(RGBD_PLANE_METHOD method = RGBD_PLANE_METHOD_DEFAULT)
+        :
+          method_(method),
+          block_size_(40),
+          min_size_(block_size_*block_size_),
+          threshold_(0.01),
+          sensor_error_a_(0),
+          sensor_error_b_(0),
+          sensor_error_c_(0)
+    {
+    }
+
+    /** Find The planes in a depth image
+     * @param points3d the 3d points organized like the depth image: rows x cols with 3 channels
+     * @param the normals for every point in the depth image
+     * @param mask An image where each pixel is labeled with the plane it belongs to
+     *        and 255 if it does not belong to any plane
+     * @param the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0, norm(a,b,c)=1
+     *        and c < 0 (so that the normal points towards the camera)
+     */
+    void
+    operator()(InputArray points3d, InputArray normals, OutputArray mask,
+               OutputArray plane_coefficients);
+
+    /** Find The planes in a depth image but without doing a normal check, which is faster but less accurate
+     * @param points3d the 3d points organized like the depth image: rows x cols with 3 channels
+     * @param mask An image where each pixel is labeled with the plane it belongs to
+     *        and 255 if it does not belong to any plane
+     * @param the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0
+     */
+    void
+    operator()(InputArray points3d, OutputArray mask, OutputArray plane_coefficients);
+
+    AlgorithmInfo*
+    info() const;
+  private:
+    /** The method to use to compute the planes */
+    int method_;
+    /** The size of the blocks to look at for a stable MSE */
+    int block_size_;
+    /** The minimum size of a cluster to be considered a plane */
+    int min_size_;
+    /** How far a point can be from a plane to belong to it (in meters) */
+    double threshold_;
+    /** coefficient of the sensor error with respect to the. All 0 by default but you want a=0.0075 for a Kinect */
+    double sensor_error_a_, sensor_error_b_, sensor_error_c_;
+  };
+
+  /** Object that contains a frame data.
+   */
+  struct CV_EXPORTS RgbdFrame
+  {
+      RgbdFrame();
+      RgbdFrame(const Mat& image, const Mat& depth, const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1);
+      virtual ~RgbdFrame();
+
+      virtual void
+      release();
+
+      int ID;
+      Mat image;
+      Mat depth;
+      Mat mask;
+      Mat normals;
+  };
+
+  /** 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 OdometryFrame : public RgbdFrame
+  {
+    /** 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.
+     */
+    enum
+    {
+      CACHE_SRC = 1, CACHE_DST = 2, CACHE_ALL = CACHE_SRC + CACHE_DST
+    };
+
+    OdometryFrame();
+    OdometryFrame(const Mat& image, const Mat& depth, const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1);
+
+    virtual void
+    release();
+
+    void
+    releasePyramids();
+
+    std::vector<Mat> pyramidImage;
+    std::vector<Mat> pyramidDepth;
+    std::vector<Mat> pyramidMask;
+
+    std::vector<Mat> pyramidCloud;
+
+    std::vector<Mat> pyramid_dI_dx;
+    std::vector<Mat> pyramid_dI_dy;
+    std::vector<Mat> pyramidTexturedMask;
+
+    std::vector<Mat> pyramidNormals;
+    std::vector<Mat> pyramidNormalsMask;
+  };
+
+  /** Base class for computation of odometry.
+   */
+  class CV_EXPORTS Odometry: public Algorithm
+  {
+  public:
+
+    /** A class of transformation*/
+    enum
+    {
+      ROTATION = 1, TRANSLATION = 2, RIGID_BODY_MOTION = 4
+    };
+
+    static inline float
+    DEFAULT_MIN_DEPTH()
+    {
+      return 0.f; // in meters
+    }
+    static inline float
+    DEFAULT_MAX_DEPTH()
+    {
+      return 4.f; // in meters
+    }
+    static inline float
+    DEFAULT_MAX_DEPTH_DIFF()
+    {
+      return 0.07f; // in meters
+    }
+    static inline float
+    DEFAULT_MAX_POINTS_PART()
+    {
+      return 0.07f; // in [0, 1]
+    }
+    static inline float
+    DEFAULT_MAX_TRANSLATION()
+    {
+      return 0.15f; // in meters
+    }
+    static inline float
+    DEFAULT_MAX_ROTATION()
+    {
+      return 15; // in degrees
+    }
+
+    /** 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 cv::Mat.
+     * The method returns true if all internal computions 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)
+     */
+    bool
+    compute(const Mat& srcImage, const Mat& srcDepth, const Mat& srcMask, const Mat& dstImage, const Mat& dstDepth,
+            const Mat& dstMask, Mat& Rt, const Mat& initRt = Mat()) const;
+
+    /** 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.).
+     */
+    bool
+    compute(Ptr<OdometryFrame>& srcFrame, Ptr<OdometryFrame>& dstFrame, Mat& Rt, const Mat& initRt = Mat()) const;
+
+    /** 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 odometry The odometry which will process the frame.
+     * @param cacheType The cache type: CACHE_SRC, CACHE_DST or CACHE_ALL.
+     */
+    virtual Size
+    prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const;
+
+  protected:
+    virtual void
+    checkParams() const = 0;
+
+    virtual bool
+    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt,
+                const Mat& initRt) const = 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 RgbdOdometry: public Odometry
+  {
+  public:
+    RgbdOdometry();
+    /** 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].
+     */
+    RgbdOdometry(const Mat& cameraMatrix, float minDepth = DEFAULT_MIN_DEPTH(), float maxDepth = DEFAULT_MAX_DEPTH(),
+                 float maxDepthDiff = DEFAULT_MAX_DEPTH_DIFF(), const std::vector<int>& iterCounts = std::vector<int>(),
+                 const std::vector<float>& minGradientMagnitudes = std::vector<float>(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(),
+                 int transformType = RIGID_BODY_MOTION);
+
+    virtual Size
+    prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const;
+
+    AlgorithmInfo*
+    info() const;
+
+  protected:
+    virtual void
+    checkParams() const;
+
+    virtual bool
+    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt,
+                const Mat& initRt) const;
+
+    // Some params have commented desired type. It's due to cv::AlgorithmInfo::addParams does not support it now.
+    /*float*/
+    double minDepth, maxDepth, maxDepthDiff;
+    /*vector<int>*/
+    Mat iterCounts;
+    /*vector<float>*/
+    Mat minGradientMagnitudes;
+    double maxPointsPart;
+
+    Mat cameraMatrix;
+    int transformType;
+
+    double maxTranslation, maxRotation;
+  };
+
+  /** Odometry based on the paper "KinectFusion: Real-Time Dense Surface Mapping and Tracking", 
+   * Richard A. Newcombe, Andrew Fitzgibbon, at al, SIGGRAPH, 2011.
+   */
+  class ICPOdometry: public Odometry
+  {
+  public:
+    ICPOdometry();
+    /** 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 pointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart
+     * @param iterCounts Count of iterations on each pyramid level.
+     */
+    ICPOdometry(const Mat& cameraMatrix, float minDepth = DEFAULT_MIN_DEPTH(), float maxDepth = DEFAULT_MAX_DEPTH(),
+                float maxDepthDiff = DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(),
+                const std::vector<int>& iterCounts = std::vector<int>(), int transformType = RIGID_BODY_MOTION);
+
+    virtual Size
+    prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const;
+
+    AlgorithmInfo*
+    info() const;
+
+  protected:
+    virtual void
+    checkParams() const;
+
+    virtual bool
+    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt,
+                const Mat& initRt) const;
+
+    // Some params have commented desired type. It's due to cv::AlgorithmInfo::addParams does not support it now.
+    /*float*/
+    double minDepth, maxDepth, maxDepthDiff;
+    /*float*/
+    double maxPointsPart;
+    /*vector<int>*/
+    Mat iterCounts;
+
+    Mat cameraMatrix;
+    int transformType;
+
+    double maxTranslation, maxRotation;
+
+    mutable cv::Ptr<cv::RgbdNormals> normalsComputer;
+  };
+
+  /** Odometry that merges RgbdOdometry and ICPOdometry by minimize sum of their energy functions.
+   */
+
+  class RgbdICPOdometry: public Odometry
+  {
+  public:
+    RgbdICPOdometry();
+    /** 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 pointsPart 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].
+     */
+    RgbdICPOdometry(const Mat& cameraMatrix, float minDepth = DEFAULT_MIN_DEPTH(), float maxDepth = DEFAULT_MAX_DEPTH(),
+                    float maxDepthDiff = DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(),
+                    const std::vector<int>& iterCounts = std::vector<int>(),
+                    const std::vector<float>& minGradientMagnitudes = std::vector<float>(),
+                    int transformType = RIGID_BODY_MOTION);
+
+    virtual Size
+    prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const;
+
+    AlgorithmInfo*
+    info() const;
+
+  protected:
+    virtual void
+    checkParams() const;
+
+    virtual bool
+    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt,
+                const Mat& initRt) const;
+
+    // Some params have commented desired type. It's due to cv::AlgorithmInfo::addParams does not support it now.
+    /*float*/
+    double minDepth, maxDepth, maxDepthDiff;
+    /*float*/
+    double maxPointsPart;
+    /*vector<int>*/
+    Mat iterCounts;
+    /*vector<float>*/
+    Mat minGradientMagnitudes;
+
+    Mat cameraMatrix;
+    int transformType;
+
+    double maxTranslation, maxRotation;
+
+    mutable cv::Ptr<cv::RgbdNormals> normalsComputer;
+  };
+
+  /** Warp the image: compute 3d points from the depth, transform them using given transformation, 
+   * then project color point cloud to an image plane. 
+   * This function can be used to visualize results of the Odometry algorithm.
+   * @param image The image (of CV_8UC1 or CV_8UC3 type)
+   * @param depth The depth (of type used in depthTo3d fuction)
+   * @param mask The mask of used pixels (of CV_8UC1), it can be empty
+   * @param Rt The transformation that will be applied to the 3d points computed from the depth
+   * @param cameraMatrix Camera matrix
+   * @param distCoeff Distortion coefficients
+   * @param warpedImage The warped image.
+   * @param warpedDepth The warped depth.
+   * @param warpedMask The warped mask.
+   */
+  CV_EXPORTS
+  void
+  warpFrame(const Mat& image, const Mat& depth, const Mat& mask, const Mat& Rt, const Mat& cameraMatrix,
+            const Mat& distCoeff, Mat& warpedImage, Mat* warpedDepth = 0, Mat* warpedMask = 0);
+
+// TODO Depth interpolation
+// Curvature
+// Get rescaleDepth return dubles if asked for
+} /* namespace cv */
+
+#endif /* __cplusplus */
+
+#endif
+
+/* End of file. */

modules/rgbd/samples/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8)
+project(map_test)
+find_package(OpenCV REQUIRED)
+
+set(SOURCES odometry_evaluation.cpp)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+add_executable(odometry_evaluation ${SOURCES} ${HEADERS})
+target_link_libraries(odometry_evaluation ${OpenCV_LIBS})

modules/rgbd/samples/odometry_evaluation.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/rgbd/rgbd.hpp>
+
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/contrib/contrib.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+
+#include <dirent.h>
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace cv;
+
+#define BILATERAL_FILTER 0// if 1 then bilateral filter will be used for the depth
+
+static
+void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
+{
+    CV_Assert( timestamps.size() == Rt.size() );
+
+    ofstream file( filename.c_str() );
+    if( !file.is_open() )
+        return;
+
+    cout.precision(4);
+    for( size_t i = 0; i < Rt.size(); i++ )
+    {
+        const Mat& Rt_curr = Rt[i];
+        if( Rt_curr.empty() )
+            continue;
+
+        CV_Assert( Rt_curr.type() == CV_64FC1 );
+
+        Mat R = Rt_curr(Rect(0,0,3,3)), rvec;
+        Rodrigues(R, rvec);
+        double alpha = norm( rvec );
+        if(alpha > DBL_MIN)
+            rvec = rvec / alpha;
+
+        double cos_alpha2 = std::cos(0.5 * alpha);
+        double sin_alpha2 = std::sin(0.5 * alpha);
+
+        rvec *= sin_alpha2;
+
+        CV_Assert( rvec.type() == CV_64FC1 );
+        // timestamp tx ty tz qx qy qz qw
+        file << timestamps[i] << " " << fixed
+             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
+             << rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl;
+
+    }
+    file.close();
+}
+
+static
+void setCameraMatrixFreiburg1(float& fx, float& fy, float& cx, float& cy)
+{
+    fx = 517.3f; fy = 516.5f; cx = 318.6f; cy = 255.3f;
+}
+
+static
+void setCameraMatrixFreiburg2(float& fx, float& fy, float& cx, float& cy)
+{
+    fx = 520.9f; fy = 521.0f; cx = 325.1f; cy = 249.7f;
+}
+
+/*
+ * This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
+ * At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory
+ * to file of format that the benchmark requires. Saved file can be used for online evaluation.
+ */
+int main(int argc, char** argv)
+{
+    if(argc != 4)
+    {
+        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP]" << endl;
+        return -1;
+    }
+    
+    vector<string> timestamps;
+    vector<Mat> Rts;
+
+    const string filename = argv[1];
+    ifstream file( filename.c_str() );
+    if( !file.is_open() )
+        return -1;
+
+    char dlmrt = '/';
+    size_t pos = filename.rfind(dlmrt);
+    string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;
+
+    const int timestampLength = 17;
+    const int rgbPathLehgth = 17+8;
+    const int depthPathLehgth = 17+10;
+
+    float fx = 525.0f, // default
+          fy = 525.0f,
+          cx = 319.5f,
+          cy = 239.5f;
+    if(filename.find("freiburg1") != string::npos)
+        setCameraMatrixFreiburg1(fx, fy, cx, cy);
+    if(filename.find("freiburg2") != string::npos)
+        setCameraMatrixFreiburg2(fx, fy, cx, cy);
+    Mat cameraMatrix = Mat::eye(3,3,CV_32FC1);
+    {
+        cameraMatrix.at<float>(0,0) = fx;
+        cameraMatrix.at<float>(1,1) = fy;
+        cameraMatrix.at<float>(0,2) = cx;
+        cameraMatrix.at<float>(1,2) = cy;
+    }
+
+    Ptr<OdometryFrame> frame_prev = new OdometryFrame(),
+                           frame_curr = new OdometryFrame();
+    Ptr<Odometry> odometry = Algorithm::create<Odometry>("RGBD." + string(argv[3]) + "Odometry");
+    if(odometry.empty())
+    {
+        cout << "Can not create Odometry algorithm. Check the passed odometry name." << endl;
+        return -1;
+    }
+    odometry->set("cameraMatrix", cameraMatrix);
+
+    TickMeter gtm;
+    int count = 0;
+    for(int i = 0; !file.eof(); i++)
+    {
+        string str;
+        std::getline(file, str);
+        if(str.empty()) break;
+        if(str.at(0) == '#') continue; /* comment */
+
+        Mat image, depth;
+        // Read one pair (rgb and depth)
+        // example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
+#if BILATERAL_FILTER
+        TickMeter tm_bilateral_filter;
+#endif
+        {
+            string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
+            string timestap = str.substr(0, timestampLength);
+            string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
+
+            image = imread(dirname + rgbFilename);
+            depth = imread(dirname + depthFilename, -1);
+
+            CV_Assert(!image.empty());
+            CV_Assert(!depth.empty());
+            CV_Assert(depth.type() == CV_16UC1);
+
+            cout << i << " " << rgbFilename << " " << depthFilename << endl;
+
+            // scale depth
+            Mat depth_flt;
+            depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
+#if not BILATERAL_FILTER
+            depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
+            depth = depth_flt;
+#else
+            tm_bilateral_filter.start();
+            depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0));
+            const double depth_sigma = 0.03;
+            const double space_sigma = 4.5;  // in pixels
+            Mat invalidDepthMask = depth_flt == 0.f;
+            depth_flt.setTo(-5*depth_sigma, invalidDepthMask);
+            bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma);
+            depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask);
+            tm_bilateral_filter.stop();
+            cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl;
+#endif
+            timestamps.push_back( timestap );
+        }
+
+        {
+            Mat gray;
+            cvtColor(image, gray, CV_BGR2GRAY);
+            frame_curr->image = gray;
+            frame_curr->depth = depth;
+            
+            Mat Rt;
+            if(!Rts.empty())
+            {
+                TickMeter tm;
+                tm.start();
+                gtm.start();
+                bool res = odometry->compute(frame_curr, frame_prev, Rt);
+                gtm.stop();
+                tm.stop();
+                count++;
+                cout << "Time " << tm.getTimeSec() << endl;
+#if BILATERAL_FILTER
+                cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl;
+#endif
+                if(!res)
+                    Rt = Mat::eye(4,4,CV_64FC1);
+            }
+
+            if( Rts.empty() )
+                Rts.push_back(Mat::eye(4,4,CV_64FC1));
+            else
+            {
+                Mat& prevRt = *Rts.rbegin();
+                cout << "Rt " << Rt << endl;
+                Rts.push_back( prevRt * Rt );
+            }
+
+            if(!frame_prev.empty())
+                frame_prev->release();
+            std::swap(frame_prev, frame_curr);
+        }
+    }
+
+    std::cout << "Average time " << gtm.getTimeSec()/count << std::endl;
+    writeResults(argv[2], timestamps, Rts);
+
+    return 0;
+}

modules/rgbd/src/depth_cleaner.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/rgbd.hpp>
+#include <iostream>
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace
+{
+  class DepthCleanerImpl
+  {
+  public:
+    DepthCleanerImpl(int window_size, int depth, cv::DepthCleaner::DEPTH_CLEANER_METHOD method)
+        :
+          depth_(depth),
+          window_size_(window_size),
+          method_(method)
+    {
+    }
+
+    virtual
+    ~DepthCleanerImpl()
+    {
+    }
+
+    virtual void
+    cache()=0;
+
+    bool
+    validate(int depth, int window_size, int method) const
+    {
+      return (window_size == window_size_) && (depth == depth_) && (method == method_);
+    }
+  protected:
+    int depth_;
+    int window_size_;
+    cv::DepthCleaner::DEPTH_CLEANER_METHOD method_;
+  };
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace
+{
+  /** Given a depth image, compute the normals as detailed in the LINEMOD paper
+   * ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
+   * by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
+   */
+  template<typename T>
+  class NIL: public DepthCleanerImpl
+  {
+  public:
+    typedef cv::Vec<T, 3> Vec3T;
+    typedef cv::Matx<T, 3, 3> Mat33T;
+
+    NIL(int window_size, int depth, cv::DepthCleaner::DEPTH_CLEANER_METHOD method)
+        :
+          DepthCleanerImpl(window_size, depth, method)
+    {
+    }
+
+    /** Compute cached data
+     */
+    virtual void
+    cache()
+    {
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    void
+    compute(const cv::Mat& depth_in, cv::Mat& depth_out) const
+    {
+      switch (depth_in.depth())
+      {
+        case CV_16U:
+        {
+          const cv::Mat_<unsigned short> &depth(depth_in);
+          cv::Mat depth_out_tmp;
+          computeImpl<unsigned short, float>(depth, depth_out_tmp, 0.001);
+          depth_out_tmp.convertTo(depth_out, CV_16U);
+          break;
+        }
+        case CV_32F:
+        {
+          const cv::Mat_<float> &depth(depth_in);
+          computeImpl<float, float>(depth, depth_out, 1);
+          break;
+        }
+        case CV_64F:
+        {
+          const cv::Mat_<double> &depth(depth_in);
+          computeImpl<double, double>(depth, depth_out, 1);
+          break;
+        }
+      }
+    }
+
+  private:
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    template<typename DepthDepth, typename ContainerDepth>
+    void
+    computeImpl(const cv::Mat_<DepthDepth> &depth_in, cv::Mat & depth_out, ContainerDepth scale) const
+    {
+      const ContainerDepth theta_mean = 30. * CV_PI / 180;
+      int rows = depth_in.rows;
+      int cols = depth_in.cols;
+
+      // Precompute some data
+      const ContainerDepth sigma_L = 0.8 + 0.035 * theta_mean / (CV_PI / 2 - theta_mean);
+      cv::Mat_<ContainerDepth> sigma_z(rows, cols);
+      for (int y = 0; y < rows; ++y)
+        for (int x = 0; x < cols; ++x)
+          sigma_z(y, x) = 0.0012 + 0.0019 * (depth_in(y, x) * scale - 0.4) * (depth_in(y, x) * scale - 0.4);
+
+      ContainerDepth difference_threshold = 10;
+      cv::Mat_<ContainerDepth> Dw_sum = cv::Mat_<ContainerDepth>::zeros(rows, cols), w_sum =
+          cv::Mat_<ContainerDepth>::zeros(rows, cols);
+      for (int y = 0; y < rows - 1; ++y)
+      {
+        // Every pixel has had the contribution of previous pixels (in a row-major way)
+        for (int x = 1; x < cols - 1; ++x)
+        {
+          for (int j = 0; j <= 1; ++j)
+            for (int i = -1; i <= 1; ++i)
+            {
+              if ((j == 0) && (i == -1))
+                continue;
+              ContainerDepth delta_u = sqrt(
+                  ContainerDepth(j) * ContainerDepth(j) + ContainerDepth(i) * ContainerDepth(i));
+              ContainerDepth delta_z;
+              if (depth_in(y, x) > depth_in(y + j, x + i))
+                delta_z = depth_in(y, x) - depth_in(y + j, x + i);
+              else
+                delta_z = depth_in(y + j, x + i) - depth_in(y, x);
+              if (delta_z < difference_threshold)
+              {
+                delta_z *= scale;
+                ContainerDepth w = exp(
+                    -delta_u * delta_u / 2 / sigma_L / sigma_L - delta_z * delta_z / 2 / sigma_z(y, x) / sigma_z(y, x));
+                w_sum(y, x) += w;
+                Dw_sum(y, x) += depth_in(y + j, x + i) * w;
+                if ((j != 0) || (i != 0))
+                {
+                  w = exp(
+                      -delta_u * delta_u / 2 / sigma_L / sigma_L - delta_z * delta_z / 2 / sigma_z(y + j, x + i)
+                                                                   / sigma_z(y + j, x + i));
+                  w_sum(y + j, x + i) += w;
+                  Dw_sum(y + j, x + i) += depth_in(y, x) * w;
+                }
+              }
+            }
+        }
+      }
+      cv::Mat(Dw_sum / w_sum).copyTo(depth_out);
+    }
+  };
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace cv
+{
+  /** Default constructor of the Algorithm class that computes normals
+   */
+  DepthCleaner::DepthCleaner(int depth, int window_size, int method_in)
+      :
+        depth_(depth),
+        window_size_(window_size),
+        method_(method_in),
+        depth_cleaner_impl_(0)
+  {
+    CV_Assert(depth == CV_16U || depth == CV_32F || depth == CV_64F);
+  }
+
+  /** Destructor
+   */
+  DepthCleaner::~DepthCleaner()
+  {
+    if (depth_cleaner_impl_ == 0)
+      return;
+    switch (method_)
+    {
+      case DEPTH_CLEANER_NIL:
+      {
+        switch (depth_)
+        {
+          case CV_16U:
+            delete reinterpret_cast<const NIL<unsigned short> *>(depth_cleaner_impl_);
+            break;
+          case CV_32F:
+            delete reinterpret_cast<const NIL<float> *>(depth_cleaner_impl_);
+            break;
+          case CV_64F:
+            delete reinterpret_cast<const NIL<double> *>(depth_cleaner_impl_);
+            break;
+        }
+        break;
+      }
+    }
+  }
+
+  void
+  DepthCleaner::initialize_cleaner_impl() const
+  {
+    CV_Assert(depth_ == CV_16U || depth_ == CV_32F || depth_ == CV_64F);
+    CV_Assert(window_size_ == 1 || window_size_ == 3 || window_size_ == 5 || window_size_ == 7);
+    CV_Assert( method_ == DEPTH_CLEANER_NIL);
+    switch (method_)
+    {
+      case (DEPTH_CLEANER_NIL):
+      {
+        switch (depth_)
+        {
+          case CV_16U:
+            depth_cleaner_impl_ = new NIL<unsigned short>(window_size_, depth_, DEPTH_CLEANER_NIL);
+            break;
+          case CV_32F:
+            depth_cleaner_impl_ = new NIL<float>(window_size_, depth_, DEPTH_CLEANER_NIL);
+            break;
+          case CV_64F:
+            depth_cleaner_impl_ = new NIL<double>(window_size_, depth_, DEPTH_CLEANER_NIL);
+            break;
+        }
+        break;
+      }
+    }
+
+    reinterpret_cast<DepthCleanerImpl *>(depth_cleaner_impl_)->cache();
+  }
+
+  /** Initializes some data that is cached for later computation
+   * If that function is not called, it will be called the first time normals are computed
+   */
+  void
+  DepthCleaner::initialize() const
+  {
+    if (depth_cleaner_impl_ == 0)
+      initialize_cleaner_impl();
+    else if (!reinterpret_cast<DepthCleanerImpl *>(depth_cleaner_impl_)->validate(depth_, window_size_, method_))
+      initialize_cleaner_impl();
+  }
+
+  /** Given a set of 3d points in a depth image, compute the normals at each point
+   * using the SRI method described in
+   * ``Fast and Accurate Computation of Surface Normals from Range Images``
+   * by H. Badino, D. Huber, Y. Park and T. Kanade
+   * @param depth depth a float depth image. Or it can be rows x cols x 3 is they are 3d points
+   * @param window_size the window size on which to compute the derivatives
+   * @return normals a rows x cols x 3 matrix
+   */
+  void
+  DepthCleaner::operator()(InputArray depth_in_array, OutputArray depth_out_array) const
+  {
+    cv::Mat depth_in = depth_in_array.getMat();
+    CV_Assert(depth_in.dims == 2);
+    CV_Assert(depth_in.channels() == 1);
+
+    depth_out_array.create(depth_in.size(), depth_);
+    cv::Mat depth_out = depth_out_array.getMat();
+
+    // Initialize the pimpl
+    initialize();
+
+    // Clean the depth
+    switch (method_)
+    {
+      case (DEPTH_CLEANER_NIL):
+      {
+        switch (depth_)
+        {
+          case CV_16U:
+            reinterpret_cast<const NIL<unsigned short> *>(depth_cleaner_impl_)->compute(depth_in, depth_out);
+            break;
+          case CV_32F:
+            reinterpret_cast<const NIL<float> *>(depth_cleaner_impl_)->compute(depth_in, depth_out);
+            break;
+          case CV_64F:
+            reinterpret_cast<const NIL<double> *>(depth_cleaner_impl_)->compute(depth_in, depth_out);
+            break;
+        }
+        break;
+      }
+    }
+  }
+}

modules/rgbd/src/depth_to_3d.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/rgbd.hpp>
+#include <limits>
+
+#include "depth_to_3d.h"
+#include "utils.h"
+
+namespace
+{
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param mask the mask of the points to consider (can be empty)
+   * @param points3d the resulting 3d points, a 3-channel matrix
+   */
+  void
+  depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const cv::Mat& v_mat, const cv::Mat& z_mat,
+                     cv::Mat& points3d)
+  {
+    CV_Assert((u_mat.size() == z_mat.size()) && (v_mat.size() == z_mat.size()));
+    if (u_mat.empty())
+      return;
+    CV_Assert((u_mat.type() == z_mat.type()) && (v_mat.type() == z_mat.type()));
+
+    //grab camera params
+    cv::Mat_<float> K;
+
+    if (in_K.depth() == CV_32F)
+      K = in_K;
+    else
+      in_K.convertTo(K, CV_32F);
+
+    float fx = K(0, 0);
+    float fy = K(1, 1);
+    float s = K(0, 1);
+    float cx = K(0, 2);
+    float cy = K(1, 2);
+
+    std::vector<cv::Mat> coordinates(3);
+
+    coordinates[0] = (u_mat - cx) / fx;
+
+    if (s != 0)
+      coordinates[0] = coordinates[0] + (-(s / fy) * v_mat + cy * s / fy) / fx;
+
+    coordinates[0] = coordinates[0].mul(z_mat);
+    coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy);
+    coordinates[2] = z_mat;
+    cv::merge(coordinates, points3d);
+  }
+
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param mask the mask of the points to consider (can be empty)
+   * @param points3d the resulting 3d points
+   */
+  void
+  depthTo3dMask(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& mask, cv::Mat& points3d)
+  {
+    // Create 3D points in one go.
+    cv::Mat_<float> u_mat, v_mat, z_mat;
+
+    cv::Mat_<uchar> uchar_mask = mask;
+
+    if (mask.depth() != (CV_8U))
+      mask.convertTo(uchar_mask, CV_8U);
+
+    // Figure out the interesting indices
+    size_t n_points;
+
+    if (depth.depth() == CV_16U)
+      n_points = convertDepthToFloat<uint16_t>(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat);
+    else if (depth.depth() == CV_16S)
+      n_points = convertDepthToFloat<int16_t>(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat);
+    else
+    {
+      CV_Assert(depth.type() == CV_32F);
+      n_points = convertDepthToFloat<float>(depth, mask, 1.0f, u_mat, v_mat, z_mat);
+    }
+
+    if (n_points == 0)
+      return;
+
+    u_mat.resize(n_points);
+    v_mat.resize(n_points);
+    z_mat.resize(n_points);
+
+    depthTo3d_from_uvz(K, u_mat, v_mat, z_mat, points3d);
+    points3d = points3d.reshape(3, 1);
+  }
+
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param points3d the resulting 3d points
+   */
+  template<typename T>
+  void
+  depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_<T>& K, cv::Mat& points3d)
+  {
+    const T inv_fx = T(1) / K(0, 0);
+    const T inv_fy = T(1) / K(1, 1);
+    const T ox = K(0, 2);
+    const T oy = K(1, 2);
+
+    // Build z
+    cv::Mat_<T> z_mat;
+    if (z_mat.depth() == in_depth.depth())
+      z_mat = in_depth;
+    else
+      rescaleDepthTemplated<T>(in_depth, z_mat);
+
+    // Pre-compute some constants
+    cv::Mat_<T> x_cache(1, in_depth.cols), y_cache(in_depth.rows, 1);
+    T* x_cache_ptr = x_cache[0], *y_cache_ptr = y_cache[0];
+    for (int x = 0; x < in_depth.cols; ++x, ++x_cache_ptr)
+      *x_cache_ptr = (x - ox) * inv_fx;
+    for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
+      *y_cache_ptr = (y - oy) * inv_fy;
+
+    y_cache_ptr = y_cache[0];
+    for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr)
+    {
+      cv::Vec<T, 3>* point = points3d.ptr<cv::Vec<T, 3> >(y);
+      const T* x_cache_ptr_end = x_cache[0] + in_depth.cols;
+      const T* depth = z_mat[y];
+      for (x_cache_ptr = x_cache[0]; x_cache_ptr != x_cache_ptr_end; ++x_cache_ptr, ++point, ++depth)
+      {
+        T z = *depth;
+        (*point)[0] = (*x_cache_ptr) * z;
+        (*point)[1] = (*y_cache_ptr) * z;
+        (*point)[2] = z;
+      }
+    }
+  }
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+namespace cv
+{
+
+  /**
+   * @param K
+   * @param depth the depth image
+   * @param u_mat the list of x coordinates
+   * @param v_mat the list of matching y coordinates
+   * @param points3d the resulting 3d points
+   */
+  void
+  depthTo3dSparse(InputArray depth_in, InputArray K_in, InputArray points_in, OutputArray points3d_out)
+  {
+    // Make sure we use foat types
+    cv::Mat points = points_in.getMat();
+    cv::Mat depth = depth_in.getMat();
+
+    cv::Mat points_float;
+    if (points.depth() != CV_32F)
+      points.convertTo(points_float, CV_32FC2);
+    else
+      points_float = points;
+
+    // Fill the depth matrix
+    cv::Mat_<float> z_mat;
+
+    if (depth.depth() == CV_16U)
+      convertDepthToFloat<uint16_t>(depth, 1.0 / 1000.0f, points_float, z_mat);
+    else if (depth.depth() == CV_16U)
+      convertDepthToFloat<int16_t>(depth, 1.0 / 1000.0f, points_float, z_mat);
+    else
+    {
+      CV_Assert(depth.type() == CV_32F);
+      convertDepthToFloat<float>(depth, 1.0f, points_float, z_mat);
+    }
+
+    std::vector<cv::Mat> channels(2);
+    cv::split(points_float, channels);
+
+    points3d_out.create(channels[0].rows, channels[0].cols, CV_32FC3);
+    cv::Mat points3d = points3d_out.getMat();
+    depthTo3d_from_uvz(K_in.getMat(), channels[0], channels[1], z_mat, points3d);
+  }
+
+  /**
+   * @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+   *              (as done with the Microsoft Kinect), otherwise, if given as CV_32F, it is assumed in meters)
+   * @param K The calibration matrix
+   * @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the
+   *        depth of `K` if `depth` is of depth CV_U
+   * @param mask the mask of the points to consider (can be empty)
+   */
+  void
+  depthTo3d(InputArray depth_in, InputArray K_in, OutputArray points3d_out, InputArray mask_in)
+  {
+    cv::Mat depth = depth_in.getMat();
+    cv::Mat K = K_in.getMat();
+    cv::Mat mask = mask_in.getMat();
+    CV_Assert(K.cols == 3 && K.rows == 3 && (K.depth() == CV_64F || K.depth()==CV_32F));
+    CV_Assert(
+        depth.type() == CV_64FC1 || depth.type() == CV_32FC1 || depth.type() == CV_16UC1 || depth.type() == CV_16SC1);
+    CV_Assert(mask.empty() || mask.channels() == 1);
+
+    // TODO figure out what to do when types are different: convert or reject ?
+    cv::Mat K_new;
+    if ((depth.depth() == CV_32F || depth.depth() == CV_64F) && depth.depth() != K.depth())
+    {
+      K.convertTo(K_new, depth.depth());
+    }
+    else
+      K_new = K;
+
+    // Create 3D points in one go.
+    if (!mask.empty())
+    {
+      cv::Mat points3d;
+      depthTo3dMask(depth, K_new, mask, points3d);
+      points3d_out.create(points3d.size(), CV_MAKETYPE(K_new.depth(), 3));
+      points3d.copyTo(points3d_out.getMat());
+    }
+    else
+    {
+      points3d_out.create(depth.size(), CV_MAKETYPE(K_new.depth(), 3));
+      cv::Mat points3d = points3d_out.getMat();
+      if (K_new.depth() == CV_64F)
+        depthTo3dNoMask<double>(depth, K_new, points3d);
+      else
+        depthTo3dNoMask<float>(depth, K_new, points3d);
+    }
+  }
+}

modules/rgbd/src/depth_to_3d.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __OPENCV_RGBD_DEPTH_TO_3D_HPP__
+#define __OPENCV_RGBD_DEPTH_TO_3D_HPP__
+
+#ifdef __cplusplus
+
+#include <opencv2/core.hpp>
+#include <limits>
+
+/**
+ * @param depth the depth image, containing depth with the value T
+ * @param the mask, containing CV_8UC1
+ */
+template <typename T>
+size_t
+convertDepthToFloat(const cv::Mat& depth, const cv::Mat& mask, float scale, cv::Mat_<float> &u_mat, cv::Mat_<float> &v_mat, cv::Mat_<float> &z_mat)
+{
+  CV_Assert (depth.size == mask.size);
+
+  cv::Size depth_size = depth.size();
+
+  cv::Mat_<uchar> uchar_mask = mask;
+
+  if (mask.depth() != CV_8U)
+    mask.convertTo(uchar_mask, CV_8U);
+
+  u_mat = cv::Mat_<float>(depth_size.area(), 1);
+  v_mat = cv::Mat_<float>(depth_size.area(), 1);
+  z_mat = cv::Mat_<float>(depth_size.area(), 1);
+
+  // Raw data from the Kinect has int
+  size_t n_points = 0;
+
+  for (int v = 0; v < depth_size.height; v++)
+  {
+    uchar* r = uchar_mask.ptr<uchar>(v, 0);
+
+    for (int u = 0; u < depth_size.width; u++, ++r)
+      if (*r)
+      {
+        u_mat(n_points, 0) = u;
+        v_mat(n_points, 0) = v;
+        T depth_i = depth.at<T>(v, u);
+
+        if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits<T>::min()) || (depth_i == std::numeric_limits<T>::max()))
+          z_mat(n_points, 0) = std::numeric_limits<float>::quiet_NaN();
+        else
+          z_mat(n_points, 0) = depth_i * scale;
+
+        ++n_points;
+      }
+  }
+
+  return n_points;
+}
+
+/**
+ * @param depth the depth image, containing depth with the value T
+ * @param the mask, containing CV_8UC1
+ */
+template <typename T>
+void
+convertDepthToFloat(const cv::Mat& depth, float scale, const cv::Mat &uv_mat, cv::Mat_<float> &z_mat)
+{
+  z_mat = cv::Mat_<float>(uv_mat.size());
+
+  // Raw data from the Kinect has int
+  float* z_mat_iter = reinterpret_cast<float*>(z_mat.data);
+
+  for (cv::Mat_<cv::Vec2f>::const_iterator uv_iter = uv_mat.begin<cv::Vec2f>(), uv_end = uv_mat.end<cv::Vec2f>();
+      uv_iter != uv_end; ++uv_iter, ++z_mat_iter)
+  {
+    T depth_i = depth.at < T > ((*uv_iter)[1], (*uv_iter)[0]);
+
+    if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits < T > ::min())
+        || (depth_i == std::numeric_limits < T > ::max()))
+      *z_mat_iter = std::numeric_limits<float>::quiet_NaN();
+    else
+      *z_mat_iter = depth_i * scale;
+  }
+}
+
+#endif /* __cplusplus */
+
+#endif
+
+/* End of file. */

modules/rgbd/src/normal.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#include <opencv2/rgbd.hpp>
+
+namespace
+{
+  /** Just compute the norm of a vector
+   * @param vec a vector of size 3 and any type T
+   * @return
+   */
+  template<typename T>
+  T
+  inline
+  norm_vec(const cv::Vec<T, 3> &vec)
+  {
+    return std::sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
+  }
+
+  /** Given 3d points, compute their distance to the origin
+   * @param points
+   * @return
+   */
+  template<typename T>
+  cv::Mat_<T>
+  computeRadius(const cv::Mat &points)
+  {
+    typedef cv::Vec<T, 3> PointT;
+
+    // Compute the
+    cv::Size size(points.cols, points.rows);
+    cv::Mat_<T> r(size);
+    if (points.isContinuous())
+      size = cv::Size(points.cols * points.rows, 1);
+    for (int y = 0; y < size.height; ++y)
+    {
+      const PointT* point = points.ptr < PointT > (y), *point_end = points.ptr < PointT > (y) + size.width;
+      T * row = r[y];
+      for (; point != point_end; ++point, ++row)
+        *row = norm_vec(*point);
+    }
+
+    return r;
+  }
+
+  // Compute theta and phi according to equation 3 of
+  // ``Fast and Accurate Computation of Surface Normals from Range Images``
+  // by H. Badino, D. Huber, Y. Park and T. Kanade
+  template<typename T>
+  void
+  computeThetaPhi(int rows, int cols, const cv::Matx<T, 3, 3>& K, cv::Mat &cos_theta, cv::Mat &sin_theta,
+                  cv::Mat &cos_phi, cv::Mat &sin_phi)
+  {
+    // Create some bogus coordinates
+    cv::Mat depth_image = K(0, 0) * cv::Mat_ < T > ::ones(rows, cols);
+    cv::Mat points3d;
+    depthTo3d(depth_image, cv::Mat(K), points3d);
+
+    typedef cv::Vec<T, 3> Vec3T;
+
+    cos_theta = cv::Mat_ < T > (rows, cols);
+    sin_theta = cv::Mat_ < T > (rows, cols);
+    cos_phi = cv::Mat_ < T > (rows, cols);
+    sin_phi = cv::Mat_ < T > (rows, cols);
+    cv::Mat r = computeRadius<T>(points3d);
+    for (int y = 0; y < rows; ++y)
+    {
+      T *row_cos_theta = cos_theta.ptr < T > (y), *row_sin_theta = sin_theta.ptr < T > (y);
+      T *row_cos_phi = cos_phi.ptr < T > (y), *row_sin_phi = sin_phi.ptr < T > (y);
+      const Vec3T * row_points = points3d.ptr < Vec3T > (y), *row_points_end = points3d.ptr < Vec3T
+          > (y) + points3d.cols;
+      const T * row_r = r.ptr < T > (y);
+      for (; row_points < row_points_end;
+          ++row_cos_theta, ++row_sin_theta, ++row_cos_phi, ++row_sin_phi, ++row_points, ++row_r)
+      {
+        // In the paper, z goes away from the camera, y goes down, x goes right
+        // OpenCV has the same conventions
+        // Theta goes from z to x (and actually goes from -pi/2 to pi/2, phi goes from z to y
+        float theta = std::atan2(row_points->val[0], row_points->val[2]);
+        *row_cos_theta = std::cos(theta);
+        *row_sin_theta = std::sin(theta);
+        float phi = std::asin(row_points->val[1] / (*row_r));
+        *row_cos_phi = std::cos(phi);
+        *row_sin_phi = std::sin(phi);
+      }
+    }
+  }
+
+  /** Modify normals to make sure they point towards the camera
+   * @param normals
+   */
+  template<typename T>
+  inline
+  void
+  signNormal(const cv::Vec<T, 3> & normal_in, cv::Vec<T, 3> & normal_out)
+  {
+    cv::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];
+  }
+  /** Modify normals to make sure they point towards the camera
+   * @param normals
+   */
+  template<typename T>
+  inline
+  void
+  signNormal(T a, T b, T c, cv::Vec<T, 3> & normal)
+  {
+    T norm = 1 / std::sqrt(a * a + b * b + c * c);
+    if (c > 0)
+    {
+      normal[0] = -a * norm;
+      normal[1] = -b * norm;
+      normal[2] = -c * norm;
+    }
+    else
+    {
+      normal[0] = a * norm;
+      normal[1] = b * norm;
+      normal[2] = c * norm;
+    }
+  }
+}
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace
+{
+  class RgbdNormalsImpl
+  {
+  public:
+    RgbdNormalsImpl(int rows, int cols, int window_size, int depth, const cv::Mat &K,
+                    cv::RgbdNormals::RGBD_NORMALS_METHOD method)
+        :
+          rows_(rows),
+          cols_(cols),
+          depth_(depth),
+          window_size_(window_size),
+          method_(method)
+    {
+      K.convertTo(K_, depth);
+      K.copyTo(K_ori_);
+    }
+
+    virtual
+    ~RgbdNormalsImpl()
+    {
+    }
+
+    virtual void
+    cache()=0;
+
+    bool
+    validate(int rows, int cols, int depth, const cv::Mat &K_ori, int window_size, int method) const
+    {
+      if ((K_ori.cols != K_ori_.cols) || (K_ori.rows != K_ori_.rows) || (K_ori.type() != K_ori_.type()))
+        return false;
+      bool K_test = !(cv::countNonZero(K_ori != K_ori_));
+      return (rows == rows_) && (cols = cols_) && (window_size == window_size_) && (depth == depth_) && (K_test)
+             && (method == method_);
+    }
+  protected:
+    int rows_, cols_, depth_;
+    cv::Mat K_, K_ori_;
+    int window_size_;
+    cv::RgbdNormals::RGBD_NORMALS_METHOD method_;
+  };
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+  /** Given a set of 3d points in a depth image, compute the normals at each point
+   * using the FALS method described in
+   * ``Fast and Accurate Computation of Surface Normals from Range Images``
+   * by H. Badino, D. Huber, Y. Park and T. Kanade
+   */
+  template<typename T>
+  class FALS: public RgbdNormalsImpl
+  {
+  public:
+    typedef cv::Matx<T, 3, 3> Mat33T;
+    typedef cv::Vec<T, 9> Vec9T;
+    typedef cv::Vec<T, 3> Vec3T;
+
+    FALS(int rows, int cols, int window_size, int depth, const cv::Mat &K, cv::RgbdNormals::RGBD_NORMALS_METHOD method)
+        :
+          RgbdNormalsImpl(rows, cols, window_size, depth, K, method)
+    {
+    }
+    ~FALS()
+    {
+    }
+
+    /** Compute cached data
+     */
+    virtual void
+    cache()
+    {
+      // Compute theta and phi according to equation 3
+      cv::Mat cos_theta, sin_theta, cos_phi, sin_phi;
+      computeThetaPhi<T>(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi);
+
+      // Compute all the v_i for every points
+      std::vector<cv::Mat> channels(3);
+      channels[0] = sin_theta.mul(cos_phi);
+      channels[1] = sin_phi;
+      channels[2] = cos_theta.mul(cos_phi);
+      cv::merge(channels, V_);
+
+      // Compute M
+      cv::Mat_<Vec9T> M(rows_, cols_);
+      Mat33T VVt;
+      const Vec3T * vec = V_[0];
+      Vec9T * M_ptr = M[0], *M_ptr_end = M_ptr + rows_ * cols_;
+      for (; M_ptr != M_ptr_end; ++vec, ++M_ptr)
+      {
+        VVt = (*vec) * vec->t();
+        *M_ptr = Vec9T(VVt.val);
+      }
+
+      cv::boxFilter(M, M, M.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false);
+
+      // Compute M's inverse
+      Mat33T M_inv;
+      M_inv_.create(rows_, cols_);
+      Vec9T * M_inv_ptr = M_inv_[0];
+      for (M_ptr = &M(0); M_ptr != M_ptr_end; ++M_inv_ptr, ++M_ptr)
+      {
+        // We have a semi-definite matrix
+        cv::invert(Mat33T(M_ptr->val), M_inv, cv::DECOMP_CHOLESKY);
+        *M_inv_ptr = Vec9T(M_inv.val);
+      }
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    virtual void
+    compute(const cv::Mat&, const cv::Mat &r, cv::Mat & normals) const
+    {
+      // Compute B
+      cv::Mat_<Vec3T> B(rows_, cols_);
+
+      const T* row_r = r.ptr < T > (0), *row_r_end = row_r + rows_ * cols_;
+      const Vec3T *row_V = V_[0];
+      Vec3T *row_B = B[0];
+      for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V)
+      {
+        if (cvIsNaN(*row_r))
+          *row_B = Vec3T();
+        else
+          *row_B = (*row_V) / (*row_r);
+      }
+
+      // Apply a box filter to B
+      cv::boxFilter(B, B, B.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false);
+
+      // compute the Minv*B products
+      row_r = r.ptr < T > (0);
+      const Vec3T * B_vec = B[0];
+      const Mat33T * M_inv = reinterpret_cast<const Mat33T *>(M_inv_.ptr(0));
+      Vec3T *normal = normals.ptr<Vec3T>(0);
+      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;
+        }
+        else
+          signNormal((*M_inv) * (*B_vec), *normal);
+    }
+
+  private:
+    cv::Mat_<Vec3T> V_;
+    cv::Mat_<Vec9T> M_inv_;
+  };
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Function that multiplies K_inv by a vector. It is just meant to speed up the product as we know
+ * that K_inv is upper triangular and K_inv(2,2)=1
+ * @param K_inv
+ * @param a
+ * @param b
+ * @param c
+ * @param res
+ */
+template<typename T, typename U>
+void
+multiply_by_K_inv(const cv::Matx<T, 3, 3> & K_inv, U a, U b, U c, cv::Vec<T, 3> &res)
+{
+  res[0] = K_inv(0, 0) * a + K_inv(0, 1) * b + K_inv(0, 2) * c;
+  res[1] = K_inv(1, 1) * b + K_inv(1, 2) * c;
+  res[2] = c;
+}
+
+namespace
+{
+  /** Given a depth image, compute the normals as detailed in the LINEMOD paper
+   * ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
+   * by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
+   */
+  template<typename T>
+  class LINEMOD: public RgbdNormalsImpl
+  {
+  public:
+    typedef cv::Vec<T, 3> Vec3T;
+    typedef cv::Matx<T, 3, 3> Mat33T;
+
+    LINEMOD(int rows, int cols, int window_size, int depth, const cv::Mat &K,
+            cv::RgbdNormals::RGBD_NORMALS_METHOD method)
+        :
+          RgbdNormalsImpl(rows, cols, window_size, depth, K, method)
+    {
+    }
+
+    /** Compute cached data
+     */
+    virtual void
+    cache()
+    {
+    }
+
+    /** Compute the normals
+     * @param r
+     * @param normals the output normals
+     */
+    void
+    compute(const cv::Mat& depth_in, cv::Mat & normals) const
+    {
+      switch (depth_in.depth())
+      {
+        case CV_16U:
+        {
+          const cv::Mat_<unsigned short> &depth(depth_in);
+          computeImpl<unsigned short, long>(depth, normals);
+          break;
+        }
+        case CV_32F:
+        {
+          const cv::Mat_<float> &depth(depth_in);
+          computeImpl<float, float>(depth, normals);
+          break;
+        }
+        case CV_64F:
+        {
+          const cv::Mat_<double> &depth(depth_in);
+          computeImpl<double, double>(depth, normals);
+          break;
+        }
+      }
+    }
+
+  private:
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    template<typename DepthDepth, typename ContainerDepth>
+    cv::Mat
+    computeImpl(const cv::Mat_<DepthDepth> &depth, cv::Mat & normals) const
+    {
+      const int r = 5; // used to be 7
+      const int sample_step = r;
+      const int square_size = ((2 * r / sample_step) + 1);
+      long offsets[square_size * square_size];
+      long offsets_x[square_size * square_size];
+      long offsets_y[square_size * square_size];
+      long offsets_x_x[square_size * square_size];
+      long offsets_x_y[square_size * square_size];
+      long offsets_y_y[square_size * square_size];
+      for (int j = -r, index = 0; j <= r; j += sample_step)
+        for (int i = -r; i <= r; i += sample_step, ++index)
+        {
+          offsets_x[index] = i;
+          offsets_y[index] = j;
+          offsets_x_x[index] = i*i;
+          offsets_x_y[index] = i*j;
+          offsets_y_y[index] = j*j;
+          offsets[index] = j * cols_ + i;
+        }
+
+      // Define K_inv by hand, just for higher accuracy
+      Mat33T K_inv = cv::Matx<T, 3, 3>::eye(), K;
+      K_.copyTo(K);
+      K_inv(0, 0) = 1.0 / K(0, 0);
+      K_inv(0, 1) = -K(0, 1) / (K(0, 0) * K(1, 1));
+      K_inv(0, 2) = (K(0, 1) * K(1, 2) - K(0, 2) * K(1, 1)) / (K(0, 0) * K(1, 1));
+      K_inv(1, 1) = 1 / K(1, 1);
+      K_inv(1, 2) = -K(1, 2) / K(1, 1);
+
+      Vec3T X1_minus_X, X2_minus_X;
+
+      ContainerDepth difference_threshold = 50;
+      for (int y = r; y < rows_ - r - 1; ++y)
+      {
+        const DepthDepth * p_line = reinterpret_cast<const DepthDepth*>(depth.ptr(y, r));
+        Vec3T *normal = normals.ptr<Vec3T>(y, r);
+
+        for (int x = r; x < cols_ - r - 1; ++x)
+        {
+          DepthDepth d = p_line[0];
+
+          // accum
+          long A[4];
+          A[0] = A[1] = A[2] = A[3] = 0;
+          ContainerDepth b[2];
+          b[0] = b[1] = 0;
+          for (unsigned int i = 0; i < square_size * square_size; ++i) {
+            // We need to cast to ContainerDepth in case we have unsigned DepthDepth
+            ContainerDepth delta = ContainerDepth(p_line[offsets[i]]) - ContainerDepth(d);
+            if (std::abs(delta) > difference_threshold)
+               continue;
+
+             A[0] += offsets_x_x[i];
+             A[1] += offsets_x_y[i];
+             A[3] += offsets_y_y[i];
+             b[0] += offsets_x[i] * delta;
+             b[1] += offsets_y[i] * delta;
+          }
+
+          // solve for the optimal gradient D of equation (8)
+          long det = A[0] * A[3] - A[1] * A[1];
+          // We should divide the following two by det, but instead, we multiply
+          // X1_minus_X and X2_minus_X by det (which does not matter as we normalize the normals)
+          // Therefore, no division is done: this is only for speedup
+          ContainerDepth dx = (A[3] * b[0] - A[1] * b[1]);
+          ContainerDepth dy = (-A[1] * b[0] + A[0] * b[1]);
+
+          // Compute the dot product
+          //Vec3T X = K_inv * Vec3T(x, y, 1) * depth(y, x);
+          //Vec3T X1 = K_inv * Vec3T(x + 1, y, 1) * (depth(y, x) + dx);
+          //Vec3T X2 = K_inv * Vec3T(x, y + 1, 1) * (depth(y, x) + dy);
+          //Vec3T nor = (X1 - X).cross(X2 - X);
+          multiply_by_K_inv(K_inv, d * det + (x + 1) * dx, y * dx, dx, X1_minus_X);
+          multiply_by_K_inv(K_inv, x * dy, d * det + (y + 1) * dy, dy, X2_minus_X);
+          Vec3T nor = X1_minus_X.cross(X2_minus_X);
+          signNormal(nor, *normal);
+
+          ++p_line;
+          ++normal;
+        }
+      }
+
+      return normals;
+    }
+  };
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace
+{
+
+  /** Given a set of 3d points in a depth image, compute the normals at each point
+   * using the SRI method described in
+   * ``Fast and Accurate Computation of Surface Normals from Range Images``
+   * by H. Badino, D. Huber, Y. Park and T. Kanade
+   */
+  template<typename T>
+  class SRI: public RgbdNormalsImpl
+  {
+  public:
+    typedef cv::Matx<T, 3, 3> Mat33T;
+    typedef cv::Vec<T, 9> Vec9T;
+    typedef cv::Vec<T, 3> Vec3T;
+
+    SRI(int rows, int cols, int window_size, int depth, const cv::Mat &K, cv::RgbdNormals::RGBD_NORMALS_METHOD method)
+        :
+          RgbdNormalsImpl(rows, cols, window_size, depth, K, method),
+          phi_step_(0),
+          theta_step_(0)
+    {
+    }
+
+    /** Compute cached data
+     */
+    virtual void
+    cache()
+    {
+      cv::Mat_<T> cos_theta, sin_theta, cos_phi, sin_phi;
+      computeThetaPhi<T>(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi);
+
+      // Create the derivative kernels
+      getDerivKernels(kx_dx_, ky_dx_, 1, 0, window_size_, true, depth_);
+      getDerivKernels(kx_dy_, ky_dy_, 0, 1, window_size_, true, depth_);
+
+      // Get the mapping function for SRI
+      float min_theta = std::asin(sin_theta(0, 0)), max_theta = std::asin(sin_theta(0, cols_ - 1));
+      float min_phi = std::asin(sin_phi(0, cols_/2-1)), max_phi = std::asin(sin_phi(rows_ - 1, cols_/2-1));
+
+      std::vector<cv::Point3f> points3d(cols_ * rows_);
+      R_hat_.create(rows_, cols_);
+      phi_step_ = float(max_phi - min_phi) / (rows_ - 1);
+      theta_step_ = float(max_theta - min_theta) / (cols_ - 1);
+      for (int phi_int = 0, k = 0; phi_int < rows_; ++phi_int)
+      {
+        float phi = min_phi + phi_int * phi_step_;
+        for (int theta_int = 0; theta_int < cols_; ++theta_int, ++k)
+        {
+          float theta = min_theta + theta_int * theta_step_;
+          // Store the 3d point to project it later
+          points3d[k] = cv::Point3f(std::sin(theta) * std::cos(phi), std::sin(phi), std::cos(theta) * std::cos(phi));
+
+          // Cache the rotation matrix and negate it
+          cv::Mat_<T> mat =
+              (cv::Mat_ < T > (3, 3) << 0, 1, 0, 0, 0, 1, 1, 0, 0) * ((cv::Mat_ < T > (3, 3) << std::cos(theta), -std::sin(
+                  theta), 0, std::sin(theta), std::cos(theta), 0, 0, 0, 1))
+              * ((cv::Mat_ < T > (3, 3) << std::cos(phi), 0, -std::sin(phi), 0, 1, 0, std::sin(phi), 0, std::cos(phi)));
+          for (unsigned char i = 0; i < 3; ++i)
+            mat(i, 1) = mat(i, 1) / std::cos(phi);
+          // The second part of the matrix is never explained in the paper ... but look at the wikipedia normal article
+          mat(0, 0) = mat(0, 0) - 2 * std::cos(phi) * std::sin(theta);
+          mat(1, 0) = mat(1, 0) - 2 * std::sin(phi);
+          mat(2, 0) = mat(2, 0) - 2 * std::cos(phi) * std::cos(theta);
+
+          R_hat_(phi_int, theta_int) = Vec9T((T*) (mat.data));
+        }
+      }
+
+      map_.create(rows_, cols_);
+      cv::projectPoints(points3d, cv::Mat(3,1,CV_32FC1,cv::Scalar::all(0.0f)), cv::Mat(3,1,CV_32FC1,cv::Scalar::all(0.0f)), K_, cv::Mat(), map_);
+      map_ = map_.reshape(2, rows_);
+      cv::convertMaps(map_, cv::Mat(), xy_, fxy_, CV_16SC2);
+
+      //map for converting from Spherical coordinate space to Euclidean space
+      euclideanMap_.create(rows_,cols_);
+      float invFx = 1.0f/K_.at<T>(0,0), cx = K_.at<T>(0,2);
+      double invFy = 1.0f/K_.at<T>(1,1), cy = K_.at<T>(1,2);
+      for (int i = 0; i < rows_; i++)
+      {
+          float y = (i - cy)*invFy;
+          for (int j = 0; j < cols_; j++)
+          {
+              float x = (j - cx)*invFx;
+              float theta = std::atan(x);
+              float phi = std::asin(y/std::sqrt(x*x+y*y+1.0f));
+
+              euclideanMap_(i,j) = cv::Vec2f((theta-min_theta)/theta_step_,(phi-min_phi)/phi_step_);
+          }
+      }
+      //convert map to 2 maps in short format for increasing speed in remap function
+      cv::convertMaps(euclideanMap_, cv::Mat(), invxy_, invfxy_, CV_16SC2);
+
+      // Update the kernels: the steps are due to the fact that derivatives will be computed on a grid where
+      // the step is not 1. Only need to do it on one dimension as it computes derivatives in only one direction
+      kx_dx_ /= theta_step_;
+      ky_dy_ /= phi_step_;
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    virtual void
+    compute(const cv::Mat& points3d, const cv::Mat &r, cv::Mat & normals) const
+    {
+      const cv::Mat_<T>& r_T(r);
+      const cv::Mat_<Vec3T> &points3d_T(points3d);
+      compute(points3d_T, r_T, normals);
+    }
+
+    /** Compute the normals
+     * @param r
+     * @return
+     */
+    void
+    compute(const cv::Mat_<Vec3T> &, const cv::Mat_<T> &r_non_interp, cv::Mat & normals_out) const
+    {
+      // Interpolate the radial image to make derivatives meaningful
+      cv::Mat_<T> r;
+      // higher quality remapping does not help here
+      cv::remap(r_non_interp, r, xy_, fxy_, CV_INTER_LINEAR);
+
+      // Compute the derivatives with respect to theta and phi
+      // TODO add bilateral filtering (as done in kinfu)
+      cv::Mat_<T> r_theta, r_phi;
+      cv::sepFilter2D(r, r_theta, r.depth(), kx_dx_, ky_dx_);
+      //current OpenCV version sometimes corrupts r matrix after second call of sepFilter2D
+      //it depends on resolution, be careful
+      cv::sepFilter2D(r, r_phi, r.depth(), kx_dy_, ky_dy_);
+
+      // Fill the result matrix
+      cv::Mat_<Vec3T> normals(rows_, cols_);
+
+      const T* r_theta_ptr = r_theta[0], *r_theta_ptr_end = r_theta_ptr + rows_ * 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];
+      for (; r_theta_ptr != r_theta_ptr_end; ++r_theta_ptr, ++r_phi_ptr, ++R, ++r_ptr, ++normal)
+      {
+        if (cvIsNaN(*r_ptr))
+        {
+          (*normal)[0] = *r_ptr;
+          (*normal)[1] = *r_ptr;
+          (*normal)[2] = *r_ptr;
+        }
+        else
+        {
+          T r_theta_over_r = (*r_theta_ptr) / (*r_ptr);
+          T r_phi_over_r = (*r_phi_ptr) / (*r_ptr);
+          // R(1,1) is 0
+          signNormal((*R)(0, 0) + (*R)(0, 1) * r_theta_over_r + (*R)(0, 2) * r_phi_over_r,
+                     (*R)(1, 0) + (*R)(1, 2) * r_phi_over_r,
+                     (*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal);
+        }
+      }
+
+      cv::remap(normals, normals_out, invxy_, invfxy_, cv::INTER_LINEAR);
+      normal = normals_out.ptr<Vec3T>(0);
+      Vec3T * normal_end = normal + rows_ * cols_;
+      for (; normal != normal_end; ++normal)
+        signNormal((*normal)[0], (*normal)[1], (*normal)[2], *normal);
+    }
+  private:
+    /** Stores R */
+    cv::Mat_<Vec9T> R_hat_;
+    float phi_step_, theta_step_;
+
+    /** Derivative kernels */
+    cv::Mat kx_dx_, ky_dx_, kx_dy_, ky_dy_;
+    /** mapping function to get an SRI image */
+    cv::Mat_<cv::Vec2f> map_;
+    cv::Mat xy_, fxy_;
+
+    cv::Mat_<cv::Vec2f> euclideanMap_;
+    cv::Mat invxy_, invfxy_;
+  };
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace cv
+{
+  /** Default constructor of the Algorithm class that computes normals
+   */
+  RgbdNormals::RgbdNormals(int rows, int cols, int depth, InputArray K_in, int window_size, int method_in)
+      :
+        rows_(rows),
+        cols_(cols),
+        depth_(depth),
+        K_(K_in.getMat()),
+        window_size_(window_size),
+        method_(method_in),
+        rgbd_normals_impl_(0)
+  {
+    CV_Assert(depth == CV_32F || depth == CV_64F);
+    CV_Assert(K_.cols == 3 && K_.rows == 3);
+  }
+
+  // Just to remove a warning
+  void delete_normals_impl(void *rgbd_normals_impl_, int method_, int depth);
+  void delete_normals_impl(void *rgbd_normals_impl_, int method_, int depth)
+  {
+    if (rgbd_normals_impl_ == 0)
+      return;
+    switch (method_)
+    {
+      case RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD:
+      {
+        if (depth == CV_32F)
+          delete reinterpret_cast<const LINEMOD<float> *>(rgbd_normals_impl_);
+        else
+          delete reinterpret_cast<const LINEMOD<double> *>(rgbd_normals_impl_);
+        break;
+      }
+      case RgbdNormals::RGBD_NORMALS_METHOD_SRI:
+      {
+        if (depth == CV_32F)
+          delete reinterpret_cast<const SRI<float> *>(rgbd_normals_impl_);
+        else
+          delete reinterpret_cast<const SRI<double> *>(rgbd_normals_impl_);
+        break;
+      }
+      case (RgbdNormals::RGBD_NORMALS_METHOD_FALS):
+      {
+        if (depth == CV_32F)
+          delete reinterpret_cast<const FALS<float> *>(rgbd_normals_impl_);
+        else
+          delete reinterpret_cast<const FALS<double> *>(rgbd_normals_impl_);
+        break;
+      }
+    }
+  }
+
+  /** Destructor
+   */
+  RgbdNormals::~RgbdNormals()
+  {
+    delete_normals_impl(rgbd_normals_impl_, method_, depth_);
+  }
+
+  void
+  RgbdNormals::initialize_normals_impl(int rows, int cols, int depth, const cv::Mat & K, int window_size,
+                                       int method_in) const
+  {
+    CV_Assert(rows > 0 && cols > 0 && (depth == CV_32F || depth == CV_64F));
+    CV_Assert(window_size == 1 || window_size == 3 || window_size == 5 || window_size == 7);
+    CV_Assert(K_.cols == 3 && K.rows == 3 && (K.depth() == CV_32F || K.depth() == CV_64F));
+    CV_Assert(
+        method_in == RGBD_NORMALS_METHOD_FALS || method_in == RGBD_NORMALS_METHOD_LINEMOD
+        || method_in == RGBD_NORMALS_METHOD_SRI);
+    switch (method_in)
+    {
+      case (RGBD_NORMALS_METHOD_FALS):
+      {
+        if (depth == CV_32F)
+          rgbd_normals_impl_ = new FALS<float>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_FALS);
+        else
+          rgbd_normals_impl_ = new FALS<double>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_FALS);
+        break;
+      }
+      case (RGBD_NORMALS_METHOD_LINEMOD):
+      {
+        if (depth == CV_32F)
+          rgbd_normals_impl_ = new LINEMOD<float>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_LINEMOD);
+        else
+          rgbd_normals_impl_ = new LINEMOD<double>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_LINEMOD);
+        break;
+      }
+      case RGBD_NORMALS_METHOD_SRI:
+      {
+        if (depth == CV_32F)
+          rgbd_normals_impl_ = new SRI<float>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_SRI);
+        else
+          rgbd_normals_impl_ = new SRI<double>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_SRI);
+        break;
+      }
+    }
+
+    reinterpret_cast<RgbdNormalsImpl *>(rgbd_normals_impl_)->cache();
+  }
+
+  /** Initializes some data that is cached for later computation
+   * If that function is not called, it will be called the first time normals are computed
+   */
+  void
+  RgbdNormals::initialize() const
+  {
+    if (rgbd_normals_impl_ == 0)
+      initialize_normals_impl(rows_, cols_, depth_, K_, window_size_, method_);
+    else if (!reinterpret_cast<RgbdNormalsImpl *>(rgbd_normals_impl_)->validate(rows_, cols_, depth_, K_, window_size_,
+                                                                                method_)) {
+      delete_normals_impl(rgbd_normals_impl_, method_, depth_);
+      initialize_normals_impl(rows_, cols_, depth_, K_, window_size_, method_);
+    }
+  }
+
+  /** Given a set of 3d points in a depth image, compute the normals at each point
+   * @param points3d_in depth a float depth image. Or it can be rows x cols x 3 is they are 3d points
+   * @param normals a rows x cols x 3 matrix
+   */
+  void
+  RgbdNormals::operator()(InputArray points3d_in, OutputArray normals_out) const
+  {
+    cv::Mat points3d_ori = points3d_in.getMat();
+
+    CV_Assert(points3d_ori.dims == 2);
+    // Either we have 3d points or a depth image
+    switch (method_)
+    {
+      case (RGBD_NORMALS_METHOD_FALS):
+      {
+        CV_Assert(points3d_ori.channels() == 3);
+        CV_Assert(points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
+        break;
+      }
+      case RGBD_NORMALS_METHOD_LINEMOD:
+      {
+        CV_Assert(
+            ((points3d_ori.channels() == 3) && (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)));
+        break;
+      }
+      case RGBD_NORMALS_METHOD_SRI:
+      {
+        CV_Assert( ((points3d_ori.channels() == 3) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
+        break;
+      }
+    }
+
+    // Initialize the pimpl
+    initialize();
+
+    // Precompute something for RGBD_NORMALS_METHOD_SRI and RGBD_NORMALS_METHOD_FALS
+    cv::Mat points3d, radius;
+    if ((method_ == RGBD_NORMALS_METHOD_SRI) || (method_ == RGBD_NORMALS_METHOD_FALS))
+    {
+      // Make the points have the right depth
+      if (points3d_ori.depth() == depth_)
+        points3d = points3d_ori;
+      else
+        points3d_ori.convertTo(points3d, depth_);
+
+      // Compute the distance to the points
+      if (depth_ == CV_32F)
+        radius = computeRadius<float>(points3d);
+      else
+        radius = computeRadius<double>(points3d);
+    }
+
+    // Get the normals
+    normals_out.create(points3d_ori.size(), CV_MAKETYPE(depth_, 3));
+    if (points3d_in.empty())
+      return;
+
+    cv::Mat normals = normals_out.getMat();
+    switch (method_)
+    {
+      case (RGBD_NORMALS_METHOD_FALS):
+      {
+        if (depth_ == CV_32F)
+          reinterpret_cast<const FALS<float> *>(rgbd_normals_impl_)->compute(points3d, radius, normals);
+        else
+          reinterpret_cast<const FALS<double> *>(rgbd_normals_impl_)->compute(points3d, radius, normals);
+        break;
+      }
+      case RGBD_NORMALS_METHOD_LINEMOD:
+      {
+        // Only focus on the depth image for LINEMOD
+        cv::Mat depth;
+        if (points3d_ori.channels() == 3)
+        {
+          std::vector<cv::Mat> channels;
+          cv::split(points3d, channels);
+          depth = channels[2];
+        }
+        else
+          depth = points3d_ori;
+
+        if (depth_ == CV_32F)
+          reinterpret_cast<const LINEMOD<float> *>(rgbd_normals_impl_)->compute(depth, normals);
+        else
+          reinterpret_cast<const LINEMOD<double> *>(rgbd_normals_impl_)->compute(depth, normals);
+        break;
+      }
+      case RGBD_NORMALS_METHOD_SRI:
+      {
+        if (depth_ == CV_32F)
+          reinterpret_cast<const SRI<float> *>(rgbd_normals_impl_)->compute(points3d, radius, normals);
+        else
+          reinterpret_cast<const SRI<double> *>(rgbd_normals_impl_)->compute(points3d, radius, normals);
+        break;
+      }
+    }
+  }
+}

modules/rgbd/src/odometry.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/rgbd.hpp>
+
+#include <iostream>
+#include <limits>
+
+#if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
+#define HAVE_EIGEN3_HERE
+#include <Eigen/Core>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <Eigen/Dense>
+#endif
+
+using namespace cv;
+
+enum
+{
+    RGBD_ODOMETRY = 1, 
+    ICP_ODOMETRY = 2, 
+    MERGED_ODOMETRY = RGBD_ODOMETRY + ICP_ODOMETRY
+};
+
+const int sobelSize = 3;
+const double sobelScale = 1./8.;
+int normalWinSize = 5;
+int normalMethod = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
+
+static inline
+void setDefaultIterCounts(Mat& iterCounts)
+{
+    iterCounts = Mat(Vec4i(7,7,7,10));
+}
+
+static inline
+void setDefaultMinGradientMagnitudes(Mat& minGradientMagnitudes)
+{
+    minGradientMagnitudes = Mat(Vec4f(10,10,10,10));
+}
+
+static
+void buildPyramidCameraMatrix(const Mat& cameraMatrix, int levels, std::vector<Mat>& pyramidCameraMatrix)
+{
+    pyramidCameraMatrix.resize(levels);
+
+    Mat cameraMatrix_dbl;
+    cameraMatrix.convertTo(cameraMatrix_dbl, CV_64FC1);
+
+    for(int i = 0; i < levels; i++)
+    {
+        Mat levelCameraMatrix = i == 0 ? cameraMatrix_dbl : 0.5f * pyramidCameraMatrix[i-1];
+        levelCameraMatrix.at<double>(2,2) = 1.;
+        pyramidCameraMatrix[i] = levelCameraMatrix;
+    }
+}
+
+static inline
+void checkImage(const Mat& image)
+{
+    if(image.empty())
+        CV_Error(CV_StsBadSize, "Image is empty.");
+    if(image.type() != CV_8UC1)
+        CV_Error(CV_StsBadSize, "Image type has to be CV_8UC1.");
+}
+
+static inline
+void checkDepth(const Mat& depth, const Size& imageSize)
+{
+    if(depth.empty())
+        CV_Error(CV_StsBadSize, "Depth is empty.");
+    if(depth.size() != imageSize)
+        CV_Error(CV_StsBadSize, "Depth has to have the size equal to the image size.");
+    if(depth.type() != CV_32FC1)
+        CV_Error(CV_StsBadSize, "Depth type has to be CV_32FC1.");
+}
+
+static inline
+void checkMask(const Mat& mask, const Size& imageSize)
+{
+    if(!mask.empty())
+    {
+        if(mask.size() != imageSize)
+            CV_Error(CV_StsBadSize, "Mask has to have the size equal to the image size.");
+        if(mask.type() != CV_8UC1)
+            CV_Error(CV_StsBadSize, "Mask type has to be CV_8UC1.");
+    }
+}
+
+static inline
+void checkNormals(const Mat& normals, const Size& depthSize)
+{
+    if(normals.size() != depthSize)
+        CV_Error(CV_StsBadSize, "Normals has to have the size equal to the depth size.");
+    if(normals.type() != CV_32FC3)
+        CV_Error(CV_StsBadSize, "Normals type has to be CV_32FC3.");
+}
+
+static
+void preparePyramidImage(const Mat& image, std::vector<Mat>& pyramidImage, size_t levelCount)
+{
+    if(!pyramidImage.empty())
+    {
+        if(pyramidImage.size() < levelCount)
+            CV_Error(CV_StsBadSize, "Levels count of pyramidImage has to be equal or less than size of iterCounts.");
+
+        CV_Assert(pyramidImage[0].size() == image.size());
+        for(size_t i = 0; i < pyramidImage.size(); i++)
+            CV_Assert(pyramidImage[i].type() == image.type());
+    }
+    else
+        buildPyramid(image, pyramidImage, levelCount - 1);
+}
+
+static
+void preparePyramidDepth(const Mat& depth, std::vector<Mat>& pyramidDepth, size_t levelCount)
+{
+    if(!pyramidDepth.empty())
+    {
+        if(pyramidDepth.size() < levelCount)
+            CV_Error(CV_StsBadSize, "Levels count of pyramidDepth has to be equal or less than size of iterCounts.");
+
+        CV_Assert(pyramidDepth[0].size() == depth.size());
+        for(size_t i = 0; i < pyramidDepth.size(); i++)
+            CV_Assert(pyramidDepth[i].type() == depth.type());
+    }
+    else
+        buildPyramid(depth, pyramidDepth, levelCount - 1);
+}
+
+static
+void preparePyramidMask(const Mat& mask, const std::vector<Mat>& pyramidDepth, float minDepth, float maxDepth,
+                        const std::vector<Mat>& pyramidNormal,
+                        std::vector<Mat>& pyramidMask)
+{
+    minDepth = std::max(0.f, minDepth);
+
+    if(!pyramidMask.empty())
+    {
+        if(pyramidMask.size() != pyramidDepth.size())
+            CV_Error(CV_StsBadSize, "Levels count of pyramidMask has to be equal to size of pyramidDepth.");
+
+        for(size_t i = 0; i < pyramidMask.size(); i++)
+        {
+            CV_Assert(pyramidMask[i].size() == pyramidDepth[i].size());
+            CV_Assert(pyramidMask[i].type() == CV_8UC1);
+        }
+    }
+    else
+    {
+        Mat validMask;
+        if(mask.empty())
+            validMask = Mat(pyramidDepth[0].size(), CV_8UC1, Scalar(255));
+        else
+            validMask = mask.clone();
+
+        buildPyramid(validMask, pyramidMask, pyramidDepth.size() - 1);
+
+        for(size_t i = 0; i < pyramidMask.size(); i++)
+        {
+            Mat levelDepth = pyramidDepth[i].clone();
+            patchNaNs(levelDepth, 0);
+
+            Mat& levelMask = pyramidMask[i];
+            levelMask &= (levelDepth > minDepth) & (levelDepth < maxDepth);
+
+            if(!pyramidNormal.empty())
+            {
+                CV_Assert(pyramidNormal[i].type() == CV_32FC3);
+                CV_Assert(pyramidNormal[i].size() == pyramidDepth[i].size());
+                Mat levelNormal = pyramidNormal[i].clone();
+
+                Mat validNormalMask = levelNormal == levelNormal; // otherwise it's Nan
+                CV_Assert(validNormalMask.type() == CV_8UC3);
+
+                std::vector<Mat> channelMasks;
+                split(validNormalMask, channelMasks);
+                validNormalMask = channelMasks[0] & channelMasks[1] & channelMasks[2];
+
+                levelMask &= validNormalMask;
+            }
+        }
+    }
+}
+
+static
+void preparePyramidCloud(const std::vector<Mat>& pyramidDepth, const Mat& cameraMatrix, std::vector<Mat>& pyramidCloud)
+{
+    if(!pyramidCloud.empty())
+    {
+        if(pyramidCloud.size() != pyramidDepth.size())
+            CV_Error(CV_StsBadSize, "Incorrect size of pyramidCloud.");
+
+        for(size_t i = 0; i < pyramidDepth.size(); i++)
+        {
+            CV_Assert(pyramidCloud[i].size() == pyramidDepth[i].size());
+            CV_Assert(pyramidCloud[i].type() == CV_32FC3);
+        }
+    }
+    else
+    {
+        std::vector<Mat> pyramidCameraMatrix;
+        buildPyramidCameraMatrix(cameraMatrix, pyramidDepth.size(), pyramidCameraMatrix);
+
+        pyramidCloud.resize(pyramidDepth.size());
+        for(size_t i = 0; i < pyramidDepth.size(); i++)
+        {
+            Mat cloud;
+            depthTo3d(pyramidDepth[i], pyramidCameraMatrix[i], cloud);
+            pyramidCloud[i] = cloud;
+        }
+    }
+}
+
+static
+void preparePyramidSobel(const std::vector<Mat>& pyramidImage, int dx, int dy, std::vector<Mat>& pyramidSobel)
+{
+    if(!pyramidSobel.empty())
+    {
+        if(pyramidSobel.size() != pyramidImage.size())
+            CV_Error(CV_StsBadSize, "Incorrect size of pyramidSobel.");
+
+        for(size_t i = 0; i < pyramidSobel.size(); i++)
+        {
+            CV_Assert(pyramidSobel[i].size() == pyramidImage[i].size());
+            CV_Assert(pyramidSobel[i].type() == CV_16SC1);
+        }
+    }
+    else
+    {
+        pyramidSobel.resize(pyramidImage.size());
+        for(size_t i = 0; i < pyramidImage.size(); i++)
+        {
+            Sobel(pyramidImage[i], pyramidSobel[i], CV_16S, dx, dy, sobelSize);
+        }
+    }
+}
+
+static
+void randomSubsetOfMask(Mat& 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 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 = subset;
+    }
+}
+
+static
+void preparePyramidTexturedMask(const std::vector<Mat>& pyramid_dI_dx, const std::vector<Mat>& pyramid_dI_dy,
+                                const std::vector<float>& minGradMagnitudes, const std::vector<Mat>& pyramidMask, double maxPointsPart,
+                                std::vector<Mat>& pyramidTexturedMask)
+{
+    if(!pyramidTexturedMask.empty())
+    {
+        if(pyramidTexturedMask.size() != pyramid_dI_dx.size())
+            CV_Error(CV_StsBadSize, "Incorrect size of pyramidTexturedMask.");
+
+        for(size_t i = 0; i < pyramidTexturedMask.size(); i++)
+        {
+            CV_Assert(pyramidTexturedMask[i].size() == pyramid_dI_dx[i].size());
+            CV_Assert(pyramidTexturedMask[i].type() == CV_8UC1);
+        }
+    }
+    else
+    {
+        const float sobelScale2_inv = 1.f / (sobelScale * sobelScale);
+        pyramidTexturedMask.resize(pyramid_dI_dx.size());
+        for(size_t i = 0; i < pyramidTexturedMask.size(); i++)
+        {
+            const float minScaledGradMagnitude2 = minGradMagnitudes[i] * minGradMagnitudes[i] * sobelScale2_inv;
+            const Mat& dIdx = pyramid_dI_dx[i];
+            const Mat& dIdy = pyramid_dI_dy[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;
+                }
+            }
+            pyramidTexturedMask[i] = texturedMask & pyramidMask[i];
+
+            randomSubsetOfMask(pyramidTexturedMask[i], maxPointsPart);
+        }
+    }
+}
+
+static
+void preparePyramidNormals(const Mat& normals, const std::vector<Mat>& pyramidDepth, std::vector<Mat>& pyramidNormals)
+{
+    if(!pyramidNormals.empty())
+    {
+        if(pyramidNormals.size() != pyramidDepth.size())
+            CV_Error(CV_StsBadSize, "Incorrect size of pyramidNormals.");
+
+        for(size_t i = 0; i < pyramidNormals.size(); i++)
+        {
+            CV_Assert(pyramidNormals[i].size() == pyramidDepth[i].size());
+            CV_Assert(pyramidNormals[i].type() == CV_32FC3);
+        }
+    }
+    else
+    {
+        buildPyramid(normals, pyramidNormals, pyramidDepth.size() - 1);
+        // renormalize normals
+        for(size_t i = 1; i < pyramidNormals.size(); i++)
+        {
+            Mat& currNormals = pyramidNormals[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(const std::vector<Mat>& pyramidNormals, const std::vector<Mat>& pyramidMask, double maxPointsPart,
+                               std::vector<Mat>& pyramidNormalsMask)
+{
+    if(!pyramidNormalsMask.empty())
+    {
+        if(pyramidNormalsMask.size() != pyramidMask.size())
+            CV_Error(CV_StsBadSize, "Incorrect size of pyramidNormalsMask.");
+
+        for(size_t i = 0; i < pyramidNormalsMask.size(); i++)
+        {
+            CV_Assert(pyramidNormalsMask[i].size() == pyramidMask[i].size());
+            CV_Assert(pyramidNormalsMask[i].type() == pyramidMask[i].type());
+        }
+    }
+    else
+    {
+        pyramidNormalsMask.resize(pyramidMask.size());
+
+        for(size_t i = 0; i < pyramidNormalsMask.size(); i++)
+        {
+            pyramidNormalsMask[i] = pyramidMask[i].clone();
+            Mat& normalsMask = pyramidNormalsMask[i];
+            for(int y = 0; y < normalsMask.rows; y++)
+            {
+                const Vec3f *normals_row = pyramidNormals[i].ptr<Vec3f>(y);
+                uchar *normalsMask_row = pyramidNormalsMask[i].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, maxPointsPart);
+        }
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+
+static
+void computeProjectiveMatrix(const Mat& ksi, Mat& Rt)
+{
+    CV_Assert(ksi.size() == Size(1,6) && ksi.type() == CV_64FC1);
+
+#ifdef HAVE_EIGEN3_HERE
+    const double* ksi_ptr = ksi.ptr<const double>();
+    Eigen::Matrix<double,4,4> twist, g;
+    twist << 0.,          -ksi_ptr[2], ksi_ptr[1],  ksi_ptr[3],
+             ksi_ptr[2],  0.,          -ksi_ptr[0], ksi_ptr[4],
+             -ksi_ptr[1], ksi_ptr[0],  0,           ksi_ptr[5],
+             0.,          0.,          0.,          0.;
+    g = twist.exp();
+
+    eigen2cv(g, Rt);
+#else
+    // TODO: check computeProjectiveMatrix when there is not eigen library, 
+    //       because it gives less accurate pose of the camera
+    Rt = Mat::eye(4, 4, CV_64FC1);
+
+    Mat R = Rt(Rect(0,0,3,3));
+    Mat rvec = ksi.rowRange(0,3);
+
+    Rodrigues(rvec, R);
+
+    Rt.at<double>(0,3) = ksi.at<double>(3);
+    Rt.at<double>(1,3) = ksi.at<double>(4);
+    Rt.at<double>(2,3) = ksi.at<double>(5);
+#endif
+}
+
+static
+void computeCorresps(const Mat& K, const Mat& K_inv, const Mat& Rt,
+                     const Mat& depth0, const Mat& validMask0,
+                     const Mat& depth1, const Mat& selectMask1, float maxDepthDiff,
+                     Mat& _corresps)
+{
+    CV_Assert(K.type() == CV_64FC1);
+    CV_Assert(K_inv.type() == CV_64FC1);
+    CV_Assert(Rt.type() == CV_64FC1);
+
+    Mat corresps(depth1.size(), CV_16SC2, Scalar::all(-1));
+    
+    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;
+    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] = KRK_inv_ptr[0] * u1;
+            KRK_inv3_u1[u1] = KRK_inv_ptr[3] * u1;
+            KRK_inv6_u1[u1] = KRK_inv_ptr[6] * u1;
+        }
+        
+        for(int v1 = 0; v1 < depth1.rows; v1++)
+        {
+            KRK_inv1_v1_plus_KRK_inv2[v1] = KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2];
+            KRK_inv4_v1_plus_KRK_inv5[v1] = KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5];
+            KRK_inv7_v1_plus_KRK_inv8[v1] = 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(u1,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;
+
+    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 = p0.x * Rt_ptr[0] + p0.y * Rt_ptr[1] + p0.z * Rt_ptr[2] + Rt_ptr[3];
+         tp0.y = p0.x * Rt_ptr[4] + p0.y * Rt_ptr[5] + p0.z * Rt_ptr[6] + Rt_ptr[7];
+         tp0.z = 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;
+
+    AutoBuffer<Point3f> transformedPoints0(correspsCount);
+    Point3f * tps0_ptr = transformedPoints0;
+
+    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 = p0.x * Rt_ptr[0] + p0.y * Rt_ptr[1] + p0.z * Rt_ptr[2] + Rt_ptr[3];
+        tp0.y = p0.x * Rt_ptr[4] + p0.y * Rt_ptr[5] + p0.z * Rt_ptr[6] + Rt_ptr[7];
+        tp0.z = 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 = cv::determinant(AtA);
+
+    if(fabs (det) < detThreshold || cvIsNaN(det) || cvIsInf(det))
+        return false;
+
+    cv::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(Mat& Rt, const Mat& initRt,
+                         const Ptr<OdometryFrame>& srcFrame,
+                         const Ptr<OdometryFrame>& dstFrame,
+                         const cv::Mat& 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;
+        break;
+    case Odometry::ROTATION:
+        transformDim = 3;
+        rgbdEquationFuncPtr = calcRgbdEquationCoeffsRotation;
+        icpEquationFuncPtr = calcICPEquationCoeffsRotation;
+        break;
+    case Odometry::TRANSLATION:
+        transformDim = 3;
+        rgbdEquationFuncPtr = calcRgbdEquationCoeffsTranslation;
+        icpEquationFuncPtr = calcICPEquationCoeffsTranslation;
+        break;
+    default:
+        CV_Error(CV_StsBadArg, "Incorrect transformation type");
+    }
+
+    const int minOverdetermScale = 20;
+    const int minCorrespsCount = minOverdetermScale * transformDim;
+
+    std::vector<Mat> pyramidCameraMatrix;
+    buildPyramidCameraMatrix(cameraMatrix, iterCounts.size(), pyramidCameraMatrix);
+
+    Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
+    Mat currRt, ksi;
+
+    bool isOk = false;
+    for(int level = iterCounts.size() - 1; level >= 0; level--)
+    {
+        const Mat& levelCameraMatrix = pyramidCameraMatrix[level];
+        const Mat& levelCameraMatrix_inv = levelCameraMatrix.inv(DECOMP_SVD);
+        const Mat& srcLevelDepth = srcFrame->pyramidDepth[level];
+        const Mat& dstLevelDepth = dstFrame->pyramidDepth[level];
+
+        const double fx = levelCameraMatrix.at<double>(0,0);
+        const double fy = levelCameraMatrix.at<double>(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);
+
+            if(method & RGBD_ODOMETRY)
+                computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
+                                srcLevelDepth, srcFrame->pyramidMask[level], dstLevelDepth, dstFrame->pyramidTexturedMask[level],
+                                maxDepthDiff, corresps_rgbd);
+
+            if(method & ICP_ODOMETRY)
+                computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv,
+                                srcLevelDepth, srcFrame->pyramidMask[level], dstLevelDepth, dstFrame->pyramidNormalsMask[level],
+                                maxDepthDiff, corresps_icp);
+
+            if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount)
+                break;
+
+            Mat AtA(transformDim, transformDim, CV_64FC1, Scalar(0)), AtB(transformDim, 1, CV_64FC1, Scalar(0));
+            if(corresps_rgbd.rows >= minCorrespsCount)
+            {
+                calcRgbdLsmMatrices(srcFrame->pyramidImage[level], srcFrame->pyramidCloud[level], resultRt,
+                                    dstFrame->pyramidImage[level], dstFrame->pyramid_dI_dx[level], dstFrame->pyramid_dI_dy[level],
+                                    corresps_rgbd, fx, fy, sobelScale,
+                                    AtA_rgbd, AtB_rgbd, rgbdEquationFuncPtr, transformDim);
+
+                AtA += AtA_rgbd;
+                AtB += AtB_rgbd;
+            }
+            if(corresps_icp.rows >= minCorrespsCount)
+            {
+                calcICPLsmMatrices(srcFrame->pyramidCloud[level], resultRt,
+                                   dstFrame->pyramidCloud[level], dstFrame->pyramidNormals[level],
+                                   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 = resultRt;
+        
+    if(isOk)
+    {
+        Mat deltaRt;
+        if(initRt.empty())
+            deltaRt = resultRt;
+        else
+            deltaRt = resultRt * initRt.inv(DECOMP_SVD);
+
+        isOk = testDeltaTransformation(deltaRt, maxTranslation, maxRotation);
+    }
+
+    return isOk;
+}
+
+template<class ImageElemType>
+static void
+warpFrameImpl(const cv::Mat& image, const Mat& depth, const Mat& mask,
+              const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
+              Mat& warpedImage, Mat* warpedDepth, Mat* warpedMask)
+{
+    CV_Assert(image.size() == depth.size());
+    
+    Mat cloud;
+    depthTo3d(depth, cameraMatrix, cloud);
+    
+    std::vector<Point2f> points2d;
+    Mat transformedCloud;
+    perspectiveTransform(cloud, transformedCloud, Rt);
+    projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
+                distCoeff, points2d);
+
+    warpedImage = Mat(image.size(), image.type(), Scalar::all(0));
+
+    Mat zBuffer(image.size(), CV_32FC1, std::numeric_limits<float>::max());
+    const Rect rect = Rect(0, 0, image.cols, image.rows);
+    
+    for (int y = 0; y < image.rows; y++)
+    {
+        //const Point3f* cloud_row = cloud.ptr<Point3f>(y);
+        const Point3f* transformedCloud_row = transformedCloud.ptr<Point3f>(y);
+        const Point2f* points2d_row = &points2d[y*image.cols];
+        const ImageElemType* image_row = image.ptr<ImageElemType>(y);
+        const uchar* mask_row = mask.empty() ? 0 : mask.ptr<uchar>(y);
+        for (int x = 0; x < image.cols; 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)
+            {
+                warpedImage.at<ImageElemType>(p2d) = image_row[x];
+                zBuffer.at<float>(p2d) = transformed_z;
+            }
+        }
+    }
+
+    if(warpedMask)
+        *warpedMask = zBuffer != std::numeric_limits<float>::max();
+
+    if(warpedDepth)
+    {
+        zBuffer.setTo(std::numeric_limits<float>::quiet_NaN(), zBuffer == std::numeric_limits<float>::max());
+        *warpedDepth = zBuffer;
+    }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace cv
+{
+
+RgbdFrame::RgbdFrame() : ID(-1)
+{}
+
+RgbdFrame::RgbdFrame(const Mat& image_in, const Mat& depth_in, const Mat& mask_in, const Mat& normals_in, int ID_in)
+    : ID(ID_in), image(image_in), depth(depth_in), mask(mask_in), normals(normals_in)
+{}
+
+RgbdFrame::~RgbdFrame()
+{}
+
+void RgbdFrame::release()
+{
+    ID = -1;
+    image.release();
+    depth.release();
+    mask.release();
+    normals.release();
+}
+
+OdometryFrame::OdometryFrame() : RgbdFrame()
+{}
+
+OdometryFrame::OdometryFrame(const Mat& image_in, const Mat& depth_in, const Mat& mask_in, const Mat& normals_in, int ID_in)
+    : RgbdFrame(image_in, depth_in, mask_in, normals_in, ID_in)
+{}
+
+void OdometryFrame::release()
+{
+    RgbdFrame::release();
+    releasePyramids();
+}
+
+void OdometryFrame::releasePyramids()
+{
+    pyramidImage.clear();
+    pyramidDepth.clear();
+    pyramidMask.clear();
+
+    pyramidCloud.clear();
+
+    pyramid_dI_dx.clear();
+    pyramid_dI_dy.clear();
+    pyramidTexturedMask.clear();
+
+    pyramidNormals.clear();
+    pyramidNormalsMask.clear();
+}
+
+bool Odometry::compute(const Mat& srcImage, const Mat& srcDepth, const Mat& srcMask,
+                       const Mat& dstImage, const Mat& dstDepth, const Mat& dstMask,
+                       Mat& Rt, const Mat& initRt) const
+{
+    Ptr<OdometryFrame> srcFrame(new OdometryFrame(srcImage, srcDepth, srcMask));
+    Ptr<OdometryFrame> dstFrame(new OdometryFrame(dstImage, dstDepth, dstMask));
+
+    return compute(srcFrame, dstFrame, Rt, initRt);
+}
+
+bool Odometry::compute(Ptr<OdometryFrame>& srcFrame, Ptr<OdometryFrame>& dstFrame, Mat& Rt, const Mat& initRt) const
+{
+    checkParams();
+
+    Size srcSize = prepareFrameCache(srcFrame, OdometryFrame::CACHE_SRC);
+    Size dstSize = prepareFrameCache(dstFrame, OdometryFrame::CACHE_DST);
+
+    if(srcSize != dstSize)
+        CV_Error(CV_StsBadSize, "srcFrame and dstFrame have to have the same size (resolution).");
+
+    return computeImpl(srcFrame, dstFrame, Rt, initRt);
+}
+
+Size Odometry::prepareFrameCache(Ptr<OdometryFrame> &frame, int /*cacheType*/) const
+{
+    if(frame == 0)
+        CV_Error(CV_StsBadArg, "Null frame pointer.\n");
+
+    return Size();
+}
+
+//
+RgbdOdometry::RgbdOdometry() :
+    minDepth(DEFAULT_MIN_DEPTH()),
+    maxDepth(DEFAULT_MAX_DEPTH()),
+    maxDepthDiff(DEFAULT_MAX_DEPTH_DIFF()),
+    maxPointsPart(DEFAULT_MAX_POINTS_PART()),
+    transformType(Odometry::RIGID_BODY_MOTION),
+    maxTranslation(DEFAULT_MAX_TRANSLATION()),
+    maxRotation(DEFAULT_MAX_ROTATION())
+
+{
+    setDefaultIterCounts(iterCounts);
+    setDefaultMinGradientMagnitudes(minGradientMagnitudes);
+}
+
+RgbdOdometry::RgbdOdometry(const Mat& _cameraMatrix,
+                           float _minDepth, float _maxDepth, float _maxDepthDiff,
+                           const std::vector<int>& _iterCounts,
+                           const std::vector<float>& _minGradientMagnitudes,
+                           float _maxPointsPart,
+                           int _transformType) :
+                           minDepth(_minDepth), maxDepth(_maxDepth), maxDepthDiff(_maxDepthDiff),
+                           iterCounts(Mat(_iterCounts).clone()),
+                           minGradientMagnitudes(Mat(_minGradientMagnitudes).clone()),
+                           maxPointsPart(_maxPointsPart),
+                           cameraMatrix(_cameraMatrix), transformType(_transformType),
+                           maxTranslation(DEFAULT_MAX_TRANSLATION()), maxRotation(DEFAULT_MAX_ROTATION())
+{
+    if(iterCounts.empty() || minGradientMagnitudes.empty())
+    {
+        setDefaultIterCounts(iterCounts);
+        setDefaultMinGradientMagnitudes(minGradientMagnitudes);
+    }
+}
+
+Size RgbdOdometry::prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const
+{
+    Odometry::prepareFrameCache(frame, cacheType);
+
+    if(frame->image.empty())
+    {
+        if(!frame->pyramidImage.empty())
+            frame->image = frame->pyramidImage[0];
+        else
+            CV_Error(CV_StsBadSize, "Image or pyramidImage have to be set.");
+    }
+    checkImage(frame->image);
+
+    if(frame->depth.empty())
+    {
+        if(!frame->pyramidDepth.empty())
+            frame->depth = frame->pyramidDepth[0];
+        else if(!frame->pyramidCloud.empty())
+        {
+            Mat cloud = frame->pyramidCloud[0];
+            std::vector<Mat> xyz;
+            cv::split(cloud, xyz);
+            frame->depth = xyz[2];
+        }
+        else
+            CV_Error(CV_StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
+    }
+    checkDepth(frame->depth, frame->image.size());
+
+    if(frame->mask.empty() && !frame->pyramidMask.empty())
+        frame->mask = frame->pyramidMask[0];
+    checkMask(frame->mask, frame->image.size());
+
+    preparePyramidImage(frame->image, frame->pyramidImage, iterCounts.total());
+
+    preparePyramidDepth(frame->depth, frame->pyramidDepth, iterCounts.total());
+
+    preparePyramidMask(frame->mask, frame->pyramidDepth, minDepth, maxDepth,
+                       frame->pyramidNormals, frame->pyramidMask);
+
+    if(cacheType & OdometryFrame::CACHE_SRC)
+        preparePyramidCloud(frame->pyramidDepth, cameraMatrix, frame->pyramidCloud);
+
+    if(cacheType & OdometryFrame::CACHE_DST)
+    {
+        preparePyramidSobel(frame->pyramidImage, 1, 0, frame->pyramid_dI_dx);
+        preparePyramidSobel(frame->pyramidImage, 0, 1, frame->pyramid_dI_dy);
+        preparePyramidTexturedMask(frame->pyramid_dI_dx, frame->pyramid_dI_dy, minGradientMagnitudes,
+                                   frame->pyramidMask, maxPointsPart, frame->pyramidTexturedMask);
+    }
+
+    return frame->image.size();
+}
+
+void RgbdOdometry::checkParams() const
+{
+    CV_Assert(maxPointsPart > 0. && maxPointsPart <= 1.);
+    CV_Assert(cameraMatrix.size() == Size(3,3) && (cameraMatrix.type() == CV_32FC1 || cameraMatrix.type() == CV_64FC1));
+    CV_Assert(minGradientMagnitudes.size() == iterCounts.size() || minGradientMagnitudes.size() == iterCounts.t().size());
+}
+
+bool RgbdOdometry::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, const Mat& initRt) const
+{
+    return RGBDICPOdometryImpl(Rt, initRt, srcFrame, dstFrame, cameraMatrix, maxDepthDiff, iterCounts, maxTranslation, maxRotation, RGBD_ODOMETRY, transformType);
+}
+
+//
+ICPOdometry::ICPOdometry() :
+    minDepth(DEFAULT_MIN_DEPTH()), maxDepth(DEFAULT_MAX_DEPTH()),
+    maxDepthDiff(DEFAULT_MAX_DEPTH_DIFF()), maxPointsPart(DEFAULT_MAX_POINTS_PART()), transformType(Odometry::RIGID_BODY_MOTION),
+    maxTranslation(DEFAULT_MAX_TRANSLATION()), maxRotation(DEFAULT_MAX_ROTATION())
+{
+    setDefaultIterCounts(iterCounts);
+}
+
+ICPOdometry::ICPOdometry(const Mat& _cameraMatrix,
+                         float _minDepth, float _maxDepth, float _maxDepthDiff,
+                         float _maxPointsPart, const std::vector<int>& _iterCounts,
+                         int _transformType) :
+                         minDepth(_minDepth), maxDepth(_maxDepth), maxDepthDiff(_maxDepthDiff),
+                         maxPointsPart(_maxPointsPart), iterCounts(Mat(_iterCounts).clone()),
+                         cameraMatrix(_cameraMatrix), transformType(_transformType),
+                         maxTranslation(DEFAULT_MAX_TRANSLATION()), maxRotation(DEFAULT_MAX_ROTATION())
+{
+    if(iterCounts.empty())
+        setDefaultIterCounts(iterCounts);
+}
+
+Size ICPOdometry::prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const
+{
+    Odometry::prepareFrameCache(frame, cacheType);
+
+    if(frame->depth.empty())
+    {
+        if(!frame->pyramidDepth.empty())
+            frame->depth = frame->pyramidDepth[0];
+        else if(!frame->pyramidCloud.empty())
+        {
+            Mat cloud = frame->pyramidCloud[0];
+            std::vector<Mat> xyz;
+            cv::split(cloud, xyz);
+            frame->depth = xyz[2];
+        }
+        else
+            CV_Error(CV_StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
+    }
+    checkDepth(frame->depth, frame->depth.size());
+
+    if(frame->mask.empty() && !frame->pyramidMask.empty())
+        frame->mask = frame->pyramidMask[0];
+    checkMask(frame->mask, frame->depth.size());
+
+    preparePyramidDepth(frame->depth, frame->pyramidDepth, iterCounts.total());
+
+    preparePyramidCloud(frame->pyramidDepth, cameraMatrix, frame->pyramidCloud);
+
+    if(cacheType & OdometryFrame::CACHE_DST)
+    {
+        if(frame->normals.empty())
+        {
+            if(!frame->pyramidNormals.empty())
+                frame->normals = frame->pyramidNormals[0];
+            else
+            {
+                if(normalsComputer.empty() ||
+                   normalsComputer->get<int>("rows") != frame->depth.rows ||
+                   normalsComputer->get<int>("cols") != frame->depth.cols ||
+                   cv::norm(normalsComputer->get<Mat>("K"), cameraMatrix) > FLT_EPSILON)
+                   normalsComputer = cv::Ptr<cv::RgbdNormals>(new RgbdNormals(frame->depth.rows, frame->depth.cols, frame->depth.depth(), cameraMatrix, normalWinSize, normalMethod));
+
+                (*normalsComputer)(frame->pyramidCloud[0], frame->normals);
+            }
+        }
+        checkNormals(frame->normals, frame->depth.size());
+
+        preparePyramidNormals(frame->normals, frame->pyramidDepth, frame->pyramidNormals);
+
+        preparePyramidMask(frame->mask, frame->pyramidDepth, minDepth, maxDepth,
+                           frame->pyramidNormals, frame->pyramidMask);
+
+        preparePyramidNormalsMask(frame->pyramidNormals, frame->pyramidMask, maxPointsPart, frame->pyramidNormalsMask);
+    }
+    else
+        preparePyramidMask(frame->mask, frame->pyramidDepth, minDepth, maxDepth,
+                           frame->pyramidNormals, frame->pyramidMask);
+
+    return frame->depth.size();
+}
+
+void ICPOdometry::checkParams() const
+{
+    CV_Assert(maxPointsPart > 0. && maxPointsPart <= 1.);
+    CV_Assert(cameraMatrix.size() == Size(3,3) && (cameraMatrix.type() == CV_32FC1 || cameraMatrix.type() == CV_64FC1));
+}
+
+bool ICPOdometry::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, const Mat& initRt) const
+{
+    return RGBDICPOdometryImpl(Rt, initRt, srcFrame, dstFrame, cameraMatrix, maxDepthDiff, iterCounts, maxTranslation, maxRotation, ICP_ODOMETRY, transformType);
+}
+
+//
+RgbdICPOdometry::RgbdICPOdometry() :
+    minDepth(DEFAULT_MIN_DEPTH()), maxDepth(DEFAULT_MAX_DEPTH()),
+    maxDepthDiff(DEFAULT_MAX_DEPTH_DIFF()), maxPointsPart(DEFAULT_MAX_POINTS_PART()), transformType(Odometry::RIGID_BODY_MOTION),
+    maxTranslation(DEFAULT_MAX_TRANSLATION()), maxRotation(DEFAULT_MAX_ROTATION())
+{
+    setDefaultIterCounts(iterCounts);
+    setDefaultMinGradientMagnitudes(minGradientMagnitudes);
+}
+
+RgbdICPOdometry::RgbdICPOdometry(const Mat& _cameraMatrix,
+                                 float _minDepth, float _maxDepth, float _maxDepthDiff,
+                                 float _maxPointsPart, const std::vector<int>& _iterCounts,
+                                 const std::vector<float>& _minGradientMagnitudes,
+                                 int _transformType) :
+                                 minDepth(_minDepth), maxDepth(_maxDepth), maxDepthDiff(_maxDepthDiff),
+                                 maxPointsPart(_maxPointsPart), iterCounts(Mat(_iterCounts).clone()),
+                                 minGradientMagnitudes(Mat(_minGradientMagnitudes).clone()),
+                                 cameraMatrix(_cameraMatrix), transformType(_transformType),
+                                 maxTranslation(DEFAULT_MAX_TRANSLATION()), maxRotation(DEFAULT_MAX_ROTATION())
+{
+    if(iterCounts.empty() || minGradientMagnitudes.empty())
+    {
+        setDefaultIterCounts(iterCounts);
+        setDefaultMinGradientMagnitudes(minGradientMagnitudes);
+    }
+}
+
+Size RgbdICPOdometry::prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const
+{
+    if(frame->image.empty())
+    {
+        if(!frame->pyramidImage.empty())
+            frame->image = frame->pyramidImage[0];
+        else
+            CV_Error(CV_StsBadSize, "Image or pyramidImage have to be set.");
+    }
+    checkImage(frame->image);
+
+    if(frame->depth.empty())
+    {
+        if(!frame->pyramidDepth.empty())
+            frame->depth = frame->pyramidDepth[0];
+        else if(!frame->pyramidCloud.empty())
+        {
+            Mat cloud = frame->pyramidCloud[0];
+            std::vector<Mat> xyz;
+            cv::split(cloud, xyz);
+            frame->depth = xyz[2];
+        }
+        else
+            CV_Error(CV_StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
+    }
+    checkDepth(frame->depth, frame->image.size());
+
+    if(frame->mask.empty() && !frame->pyramidMask.empty())
+        frame->mask = frame->pyramidMask[0];
+    checkMask(frame->mask, frame->image.size());
+
+    preparePyramidImage(frame->image, frame->pyramidImage, iterCounts.total());
+
+    preparePyramidDepth(frame->depth, frame->pyramidDepth, iterCounts.total());
+
+    preparePyramidCloud(frame->pyramidDepth, cameraMatrix, frame->pyramidCloud);
+
+    if(cacheType & OdometryFrame::CACHE_DST)
+    {
+        if(frame->normals.empty())
+        {
+            if(!frame->pyramidNormals.empty())
+                frame->normals = frame->pyramidNormals[0];
+            else
+            {
+                if(normalsComputer.empty() ||
+                   normalsComputer->get<int>("rows") != frame->depth.rows ||
+                   normalsComputer->get<int>("cols") != frame->depth.cols ||
+                   cv::norm(normalsComputer->get<Mat>("K"), cameraMatrix) > FLT_EPSILON)
+                   normalsComputer = cv::Ptr<cv::RgbdNormals>(new RgbdNormals(frame->depth.rows, frame->depth.cols, frame->depth.depth(), cameraMatrix, normalWinSize, normalMethod));
+
+                (*normalsComputer)(frame->pyramidCloud[0], frame->normals);
+            }
+        }
+        checkNormals(frame->normals, frame->depth.size());
+
+        preparePyramidNormals(frame->normals, frame->pyramidDepth, frame->pyramidNormals);
+
+        preparePyramidMask(frame->mask, frame->pyramidDepth, minDepth, maxDepth,
+                           frame->pyramidNormals, frame->pyramidMask);
+
+        preparePyramidSobel(frame->pyramidImage, 1, 0, frame->pyramid_dI_dx);
+        preparePyramidSobel(frame->pyramidImage, 0, 1, frame->pyramid_dI_dy);
+        preparePyramidTexturedMask(frame->pyramid_dI_dx, frame->pyramid_dI_dy,
+                                   minGradientMagnitudes, frame->pyramidMask,
+                                   maxPointsPart, frame->pyramidTexturedMask);
+
+        preparePyramidNormalsMask(frame->pyramidNormals, frame->pyramidMask, maxPointsPart, frame->pyramidNormalsMask);
+    }
+    else
+        preparePyramidMask(frame->mask, frame->pyramidDepth, minDepth, maxDepth,
+                           frame->pyramidNormals, frame->pyramidMask);
+
+    return frame->image.size();
+}
+
+void RgbdICPOdometry::checkParams() const
+{
+    CV_Assert(maxPointsPart > 0. && maxPointsPart <= 1.);
+    CV_Assert(cameraMatrix.size() == Size(3,3) && (cameraMatrix.type() == CV_32FC1 || cameraMatrix.type() == CV_64FC1));
+    CV_Assert(minGradientMagnitudes.size() == iterCounts.size() || minGradientMagnitudes.size() == iterCounts.t().size());
+}
+
+bool RgbdICPOdometry::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, const Mat& initRt) const
+{
+    return RGBDICPOdometryImpl(Rt, initRt, srcFrame, dstFrame, cameraMatrix, maxDepthDiff, iterCounts,  maxTranslation, maxRotation, MERGED_ODOMETRY, transformType);
+}
+
+//
+
+void
+warpFrame(const Mat& image, const Mat& depth, const Mat& mask,
+          const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
+          Mat& warpedImage, Mat* warpedDepth, Mat* warpedMask)
+{
+    if(image.type() == CV_8UC1)
+        warpFrameImpl<uchar>(image, depth, mask, Rt, cameraMatrix, distCoeff, warpedImage, warpedDepth, warpedMask);
+    else if(image.type() == CV_8UC3)
+        warpFrameImpl<Point3_<uchar> >(image, depth, mask, Rt, cameraMatrix, distCoeff, warpedImage, warpedDepth, warpedMask);
+    else
+        CV_Error(CV_StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3");
+}
+} // namespace cv

modules/rgbd/src/plane.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+/** This is an implementation of a fast plane detection loosely inspired by
+ * Fast Plane Detection and Polygonalization in noisy 3D Range Images
+ * Jann Poppinga, Narunas Vaskevicius, Andreas Birk, and Kaustubh Pathak
+ * and the follow-up
+ * Fast Plane Detection for SLAM from Noisy Range Images in
+ * Both Structured and Unstructured Environments
+ * Junhao Xiao, Jianhua Zhang and Jianwei Zhang
+ * Houxiang Zhang and Hans Petter Hildre
+ */
+
+#include <list>
+#include <set>
+
+#include <opencv2/rgbd.hpp>
+
+/** Structure defining a plane. The notations are from the second paper */
+class PlaneBase
+{
+public:
+  PlaneBase(const cv::Vec3f & m, const cv::Vec3f &n_in, int index)
+      :
+        index_(index),
+        n_(n_in),
+        m_sum_(cv::Vec3f(0, 0, 0)),
+        m_(m),
+        Q_(cv::Matx33f::zeros()),
+        mse_(0),
+        K_(0)
+  {
+    UpdateD();
+  }
+
+  virtual
+  ~PlaneBase()
+  {
+  }
+
+  /** Compute the distance to the plane. This will be implemented by the children to take into account different
+   * sensor models
+   * @param p_j
+   * @return
+   */
+  virtual
+  float
+  distance(const cv::Vec3f& p_j) const = 0;
+
+  /** The d coefficient in the plane equation ax+by+cz+d = 0
+   * @return
+   */
+  inline float
+  d() const
+  {
+    return d_;
+  }
+
+  /** The normal to the plane
+   * @return the normal to the plane
+   */
+  const cv::Vec3f &
+  n() const
+  {
+    return n_;
+  }
+
+  /** Update the different coefficients of the plane, based on the new statistics
+   */
+  void
+  UpdateParameters()
+  {
+    if (empty())
+      return;
+    m_ = m_sum_ / K_;
+    // Compute C
+    cv::Matx33f C = Q_ - m_sum_ * m_.t();
+
+    // Compute n
+    cv::SVD svd(C);
+    n_ = cv::Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2));
+    mse_ = svd.w.at<float>(2) / K_;
+
+    UpdateD();
+  }
+
+  /** Update the different sum of point and sum of point*point.t()
+   */
+  void
+  UpdateStatistics(const cv::Vec3f & point, const cv::Matx33f & Q_local)
+  {
+    m_sum_ += point;
+    Q_ += Q_local;
+    ++K_;
+  }
+
+  inline size_t
+  empty() const
+  {
+    return K_ == 0;
+  }
+
+  inline int
+  K() const
+  {
+    return K_;
+  }
+/** The index of the plane */
+  int index_;
+protected:
+  /** The 4th coefficient in the plane equation ax+by+cz+d = 0 */
+  float d_;
+  /** Normal of the plane */
+  cv::Vec3f n_;
+private:
+  inline void
+  UpdateD()
+  {
+    d_ = -m_.dot(n_);
+  }
+  /** The sum of the points */
+  cv::Vec3f m_sum_;
+  /** The mean of the points */
+  cv::Vec3f m_;
+  /** The sum of pi * pi^\top */
+  cv::Matx33f Q_;
+  /** The different matrices we need to update */
+  cv::Matx33f C_;
+  float mse_;
+  /** the number of points that form the plane */
+  int K_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Basic planar child, with no sensor error model
+ */
+class Plane: public PlaneBase
+{
+public:
+  Plane(const cv::Vec3f & m, const cv::Vec3f &n_in, int index)
+      :
+        PlaneBase(m, n_in, index)
+  {
+  }
+
+  /** The computed distance is perfect in that case
+   * @param p_j the point to compute its distance to
+   * @return
+   */
+  float
+  distance(const cv::Vec3f& p_j) const
+  {
+    return std::abs(float(p_j.dot(n_) + d_));
+  }
+};
+
+/** Planar child with a quadratic error model
+ */
+class PlaneABC: public PlaneBase
+{
+public:
+  PlaneABC(const cv::Vec3f & m, const cv::Vec3f &n_in, int index, float sensor_error_a, float sensor_error_b,
+           float sensor_error_c)
+      :
+        PlaneBase(m, n_in, index),
+        sensor_error_a_(sensor_error_a),
+        sensor_error_b_(sensor_error_b),
+        sensor_error_c_(sensor_error_c)
+  {
+  }
+
+  /** The distance is now computed by taking the sensor error into account */
+  inline
+  float
+  distance(const cv::Vec3f& p_j) const
+  {
+    float cst = p_j.dot(n_) + d_;
+    float err = sensor_error_a_ * p_j[2] * p_j[2] + sensor_error_b_ * p_j[2] + sensor_error_c_;
+    if (((cst - n_[2] * err <= 0) && (cst + n_[2] * err >= 0)) || ((cst + n_[2] * err <= 0) && (cst - n_[2] * err >= 0)))
+      return 0;
+    return std::min(std::abs(cst - err), std::abs(cst + err));
+  }
+private:
+  float sensor_error_a_;
+  float sensor_error_b_;
+  float sensor_error_c_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** The PlaneGrid contains statistic about the individual tiles
+ */
+class PlaneGrid
+{
+public:
+  PlaneGrid(const cv::Mat_<cv::Vec3f> & points3d, int block_size)
+      :
+        block_size_(block_size)
+  {
+    // Figure out some dimensions
+    int mini_rows = points3d.rows / block_size;
+    if (points3d.rows % block_size != 0)
+      ++mini_rows;
+
+    int mini_cols = points3d.cols / block_size;
+    if (points3d.cols % block_size != 0)
+      ++mini_cols;
+
+    // Compute all the interesting quantities
+    m_.create(mini_rows, mini_cols);
+    n_.create(mini_rows, mini_cols);
+    Q_.create(points3d.rows, points3d.cols);
+    mse_.create(mini_rows, mini_cols);
+    for (int y = 0; y < mini_rows; ++y)
+      for (int x = 0; x < mini_cols; ++x)
+      {
+        // Update the tiles
+        cv::Matx33f Q = cv::Matx33f::zeros();
+        cv::Vec3f m = cv::Vec3f(0, 0, 0);
+        int K = 0;
+        for (int j = y * block_size; j < std::min((y + 1) * block_size, points3d.rows); ++j)
+        {
+          const cv::Vec3f * vec = points3d.ptr < cv::Vec3f > (j, x * block_size), *vec_end;
+          float * pointpointt = reinterpret_cast<float*>(Q_.ptr < cv::Vec<float, 9> > (j, x * block_size));
+          if (x == mini_cols - 1)
+            vec_end = points3d.ptr < cv::Vec3f > (j, points3d.cols - 1) + 1;
+          else
+            vec_end = vec + block_size;
+          for (; vec != vec_end; ++vec, pointpointt += 9)
+          {
+            if (cvIsNaN(vec->val[0]))
+              continue;
+            // Fill point*point.t()
+            *pointpointt = vec->val[0] * vec->val[0];
+            *(pointpointt + 1) = vec->val[0] * vec->val[1];
+            *(pointpointt + 2) = vec->val[0] * vec->val[2];
+            *(pointpointt + 3) = *(pointpointt + 1);
+            *(pointpointt + 4) = vec->val[1] * vec->val[1];
+            *(pointpointt + 5) = vec->val[1] * vec->val[2];
+            *(pointpointt + 6) = *(pointpointt + 2);
+            *(pointpointt + 7) = *(pointpointt + 5);
+            *(pointpointt + 8) = vec->val[2] * vec->val[2];
+
+            Q += *reinterpret_cast<cv::Matx33f*>(pointpointt);
+            m += (*vec);
+            ++K;
+          }
+        }
+        if (K == 0)
+        {
+          mse_(y, x) = std::numeric_limits<float>::max();
+          continue;
+        }
+
+        m /= K;
+        m_(y, x) = m;
+
+        // Compute C
+        cv::Matx33f C = Q - K * m * m.t();
+
+        // Compute n
+        cv::SVD svd(C);
+        n_(y, x) = cv::Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2));
+        mse_(y, x) = svd.w.at<float>(2) / K;
+      }
+  }
+
+  /** The size of the block */
+  int block_size_;
+  cv::Mat_<cv::Vec3f> m_;
+  cv::Mat_<cv::Vec3f> n_;
+  cv::Mat_<cv::Vec<float, 9> > Q_;
+  cv::Mat_<float> mse_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class TileQueue
+{
+public:
+  struct PlaneTile
+  {
+    PlaneTile(int x, int y, float mse)
+        :
+          x_(x),
+          y_(y),
+          mse_(mse)
+    {
+    }
+
+    bool
+    operator<(const PlaneTile &tile2) const
+    {
+      return mse_ < tile2.mse_;
+    }
+
+    int x_;
+    int y_;
+    float mse_;
+  };
+
+  TileQueue(const PlaneGrid &plane_grid)
+  {
+    done_tiles_ = cv::Mat_<unsigned char>::zeros(plane_grid.mse_.rows, plane_grid.mse_.cols);
+    tiles_.clear();
+    for (int y = 0; y < plane_grid.mse_.rows; ++y)
+      for (int x = 0; x < plane_grid.mse_.cols; ++x)
+        if (plane_grid.mse_(y, x) != std::numeric_limits<float>::max())
+          // Update the tiles
+          tiles_.push_back(PlaneTile(x, y, plane_grid.mse_(y, x)));
+    // Sort tiles by MSE
+    tiles_.sort();
+  }
+
+  bool
+  empty()
+  {
+    while (!tiles_.empty())
+    {
+      const PlaneTile & tile = tiles_.front();
+      if (done_tiles_(tile.y_, tile.x_))
+        tiles_.pop_front();
+      else
+        break;
+    }
+    return tiles_.empty();
+  }
+
+  const PlaneTile &
+  front() const
+  {
+    return tiles_.front();
+  }
+
+  void
+  remove(int y, int x)
+  {
+    done_tiles_(y, x) = 1;
+  }
+private:
+  /** The list of tiles ordered from most planar to least */
+  std::list<PlaneTile> tiles_;
+  /** contains 1 when the tiles has been studied, 0 otherwise */
+  cv::Mat_<unsigned char> done_tiles_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class InlierFinder
+{
+public:
+  InlierFinder(float err, const cv::Mat_<cv::Vec3f> & points3d, const cv::Mat_<cv::Vec3f> & normals,
+               unsigned char plane_index, int block_size)
+      :
+        err_(err),
+        points3d_(points3d),
+        normals_(normals),
+        plane_index_(plane_index),
+        block_size_(block_size)
+  {
+  }
+
+  void
+  Find(const PlaneGrid &plane_grid, cv::Ptr<PlaneBase> & plane, TileQueue & tile_queue,
+       std::set<TileQueue::PlaneTile> & neighboring_tiles, cv::Mat_<unsigned char> & overall_mask,
+       cv::Mat_<unsigned char> & plane_mask)
+  {
+    // Do not use reference as we pop the from later on
+    TileQueue::PlaneTile tile = *(neighboring_tiles.begin());
+
+    // Figure the part of the image to look at
+    cv::Range range_x, range_y;
+    int x = tile.x_ * block_size_, y = tile.y_ * block_size_;
+
+    if (tile.x_ == plane_mask.cols - 1)
+      range_x = cv::Range(x, overall_mask.cols);
+    else
+      range_x = cv::Range(x, x + block_size_);
+
+    if (tile.y_ == plane_mask.rows - 1)
+      range_y = cv::Range(y, overall_mask.rows);
+    else
+      range_y = cv::Range(y, y + block_size_);
+
+    int n_valid_points = 0;
+    for (int yy = range_y.start; yy != range_y.end; ++yy)
+    {
+      uchar* data = overall_mask.ptr(yy, range_x.start), *data_end = data + range_x.size();
+      const cv::Vec3f* point = points3d_.ptr < cv::Vec3f > (yy, range_x.start);
+      const cv::Matx33f* Q_local = reinterpret_cast<const cv::Matx33f *>(plane_grid.Q_.ptr < cv::Vec<float, 9>
+          > (yy, range_x.start));
+
+      // Depending on whether you have a normal, check it
+      if (!normals_.empty())
+      {
+        const cv::Vec3f* normal = normals_.ptr < cv::Vec3f > (yy, range_x.start);
+        for (; data != data_end; ++data, ++point, ++normal, ++Q_local)
+        {
+          // Don't do anything if the point already belongs to another plane
+          if (cvIsNaN(point->val[0]) || ((*data) != 255))
+            continue;
+
+          // If the point is close enough to the plane
+          if (plane->distance(*point) < err_)
+          {
+            // make sure the normals are similar to the plane
+            if (std::abs(plane->n().dot(*normal)) > 0.3)
+            {
+              // The point now belongs to the plane
+              plane->UpdateStatistics(*point, *Q_local);
+              *data = plane_index_;
+              ++n_valid_points;
+            }
+          }
+        }
+      }
+      else
+      {
+        for (; data != data_end; ++data, ++point, ++Q_local)
+        {
+          // Don't do anything if the point already belongs to another plane
+          if (cvIsNaN(point->val[0]) || ((*data) != 255))
+            continue;
+
+          // If the point is close enough to the plane
+          if (plane->distance(*point) < err_)
+          {
+            // The point now belongs to the plane
+            plane->UpdateStatistics(*point, *Q_local);
+            *data = plane_index_;
+            ++n_valid_points;
+          }
+        }
+      }
+    }
+
+    plane->UpdateParameters();
+
+    // Mark the front as being done and pop it
+    if (n_valid_points > (range_x.size() * range_y.size()) / 2)
+      tile_queue.remove(tile.y_, tile.x_);
+    plane_mask(tile.y_, tile.x_) = 1;
+    neighboring_tiles.erase(neighboring_tiles.begin());
+
+    // Add potential neighbors of the tile
+    std::vector<std::pair<int, int> > pairs;
+    if (tile.x_ > 0)
+      for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.start, range_x.start), *val_end = val
+          + range_y.size() * overall_mask.step; val != val_end; val += overall_mask.step)
+        if (*val == plane_index_)
+        {
+          pairs.push_back(std::pair<int, int>(tile.x_ - 1, tile.y_));
+          break;
+        }
+    if (tile.x_ < plane_mask.cols - 1)
+      for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.start, range_x.end - 1), *val_end = val
+          + range_y.size() * overall_mask.step; val != val_end; val += overall_mask.step)
+        if (*val == plane_index_)
+        {
+          pairs.push_back(std::pair<int, int>(tile.x_ + 1, tile.y_));
+          break;
+        }
+    if (tile.y_ > 0)
+      for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.start, range_x.start), *val_end = val
+          + range_x.size(); val != val_end; ++val)
+        if (*val == plane_index_)
+        {
+          pairs.push_back(std::pair<int, int>(tile.x_, tile.y_ - 1));
+          break;
+        }
+    if (tile.y_ < plane_mask.rows - 1)
+      for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.end - 1, range_x.start), *val_end = val
+          + range_x.size(); val != val_end; ++val)
+        if (*val == plane_index_)
+        {
+          pairs.push_back(std::pair<int, int>(tile.x_, tile.y_ + 1));
+          break;
+        }
+
+    for (unsigned char i = 0; i < pairs.size(); ++i)
+      if (!plane_mask(pairs[i].second, pairs[i].first))
+        neighboring_tiles.insert(
+            TileQueue::PlaneTile(pairs[i].first, pairs[i].second, plane_grid.mse_(pairs[i].second, pairs[i].first)));
+  }
+
+private:
+  float err_;
+  const cv::Mat_<cv::Vec3f> & points3d_;
+  const cv::Mat_<cv::Vec3f> & normals_;
+  unsigned char plane_index_;
+  /** THe block size as defined in the main algorithm */
+  int block_size_;
+}
+;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+namespace cv
+{
+  void
+  RgbdPlane::operator()(InputArray points3d_in, OutputArray mask_out, OutputArray plane_coefficients)
+  {
+    this->operator()(points3d_in, cv::Mat(), mask_out, plane_coefficients);
+  }
+
+  void
+  RgbdPlane::operator()(InputArray points3d_in, InputArray normals_in, OutputArray mask_out,
+                        OutputArray plane_coefficients_out)
+  {
+    cv::Mat_<cv::Vec3f> points3d, normals;
+    if (points3d_in.depth() == CV_32F)
+      points3d = points3d_in.getMat();
+    else
+      points3d_in.getMat().convertTo(points3d, CV_32F);
+    if (!normals_in.empty())
+    {
+      if (normals_in.depth() == CV_32F)
+        normals = normals_in.getMat();
+      else
+        normals_in.getMat().convertTo(normals, CV_32F);
+    }
+
+    // Pre-computations
+    mask_out.create(points3d.size(), CV_8U);
+    cv::Mat mask_out_mat = mask_out.getMat();
+    cv::Mat_<unsigned char> mask_out_uc = (cv::Mat_<unsigned char>&) mask_out_mat;
+    mask_out_uc.setTo(255);
+    PlaneGrid plane_grid(points3d, block_size_);
+    TileQueue plane_queue(plane_grid);
+    size_t index_plane = 0;
+
+    std::vector<cv::Vec4f> plane_coefficients;
+    float mse_min = threshold_ * threshold_;
+
+    while (!plane_queue.empty())
+    {
+      // Get the first tile if it's good enough
+      const TileQueue::PlaneTile front_tile = plane_queue.front();
+      if (front_tile.mse_ > mse_min)
+        break;
+
+      InlierFinder inlier_finder(threshold_, points3d, normals, index_plane, block_size_);
+
+      // Construct the plane for the first tile
+      int x = front_tile.x_, y = front_tile.y_;
+      const cv::Vec3f & n = plane_grid.n_(y, x);
+      cv::Ptr<PlaneBase> plane;
+      if ((sensor_error_a_ == 0) && (sensor_error_b_ == 0) && (sensor_error_c_ == 0))
+        plane = cv::Ptr<PlaneBase>(new Plane(plane_grid.m_(y, x), n, index_plane));
+      else
+        plane = cv::Ptr<PlaneBase>(new PlaneABC(plane_grid.m_(y, x), n, index_plane, sensor_error_a_, sensor_error_b_, sensor_error_c_));
+
+      cv::Mat_<unsigned char> plane_mask = cv::Mat_<unsigned char>::zeros(points3d.rows / block_size_,
+                                                                          points3d.cols / block_size_);
+      std::set<TileQueue::PlaneTile> neighboring_tiles;
+      neighboring_tiles.insert(front_tile);
+      plane_queue.remove(front_tile.y_, front_tile.x_);
+
+      // Process all the neighboring tiles
+      while (!neighboring_tiles.empty())
+        inlier_finder.Find(plane_grid, plane, plane_queue, neighboring_tiles, mask_out_uc, plane_mask);
+
+      // Don't record the plane if it's empty
+      if (plane->empty())
+        continue;
+      // Don't record the plane if it's smaller than asked
+      if (plane->K() < min_size_) {
+        // Reset the plane index in the mask
+        for (y = 0; y < plane_mask.rows; ++y)
+          for (x = 0; x < plane_mask.cols; ++x) {
+            if (!plane_mask(y, x))
+              continue;
+            // Go over the tile
+            for (int yy = y * block_size_;
+                yy < std::min((y + 1) * block_size_, mask_out_uc.rows); ++yy) {
+              uchar* data = mask_out_uc.ptr(yy, x * block_size_);
+              uchar* data_end = data
+                  + std::min(block_size_,
+                      mask_out_uc.cols - x * block_size_);
+              for (; data != data_end; ++data) {
+                if (*data == index_plane)
+                  *data = 255;
+              }
+            }
+          }
+        continue;
+      }
+
+      ++index_plane;
+      if (index_plane >= 255)
+        break;
+      cv::Vec4f coeffs(plane->n()[0], plane->n()[1], plane->n()[2], plane->d());
+      if (coeffs(2) > 0)
+        coeffs = -coeffs;
+      plane_coefficients.push_back(coeffs);
+    };
+
+    // Fill the plane coefficients
+    if (plane_coefficients.empty())
+      return;
+    plane_coefficients_out.create(plane_coefficients.size(), 1, CV_32FC4);
+    cv::Mat plane_coefficients_mat = plane_coefficients_out.getMat();
+    float* data = plane_coefficients_mat.ptr<float>(0);
+    for(size_t i=0; i<plane_coefficients.size(); ++i)
+      for(uchar j=0; j<4; ++j, ++data)
+        *data = plane_coefficients[i][j];
+  }
+}

modules/rgbd/src/rgbd_init.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/core.hpp>
+#include <opencv2/rgbd.hpp>
+#include "opencv2/core/utility.hpp"
+#include "opencv2/core/private.hpp"
+
+namespace cv
+{
+  CV_INIT_ALGORITHM(DepthCleaner, "RGBD.DepthCleaner",
+      obj.info()->addParam(obj, "window_size", obj.window_size_);
+      obj.info()->addParam(obj, "depth", obj.depth_);
+      obj.info()->addParam(obj, "method", obj.method_))
+
+  CV_INIT_ALGORITHM(RgbdNormals, "RGBD.RgbdNormals",
+      obj.info()->addParam(obj, "rows", obj.rows_);
+      obj.info()->addParam(obj, "cols", obj.cols_);
+      obj.info()->addParam(obj, "window_size", obj.window_size_);
+      obj.info()->addParam(obj, "depth", obj.depth_);
+      obj.info()->addParam(obj, "K", obj.K_);
+      obj.info()->addParam(obj, "method", obj.method_))
+
+  CV_INIT_ALGORITHM(RgbdPlane, "RGBD.RgbdPlane",
+      obj.info()->addParam(obj, "block_size", obj.block_size_);
+      obj.info()->addParam(obj, "min_size", obj.min_size_);
+      obj.info()->addParam(obj, "method", obj.method_);
+      obj.info()->addParam(obj, "threshold", obj.threshold_);
+      obj.info()->addParam(obj, "sensor_error_a", obj.sensor_error_a_);
+      obj.info()->addParam(obj, "sensor_error_b", obj.sensor_error_b_);
+      obj.info()->addParam(obj, "sensor_error_c", obj.sensor_error_c_))
+
+  CV_INIT_ALGORITHM(RgbdOdometry, "RGBD.RgbdOdometry",
+      obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix);
+      obj.info()->addParam(obj, "minDepth", obj.minDepth);
+      obj.info()->addParam(obj, "maxDepth", obj.maxDepth);
+      obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff);
+      obj.info()->addParam(obj, "iterCounts", obj.iterCounts);
+      obj.info()->addParam(obj, "minGradientMagnitudes", obj.minGradientMagnitudes);
+      obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart);
+      obj.info()->addParam(obj, "transformType", obj.transformType);
+      obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation);
+      obj.info()->addParam(obj, "maxRotation", obj.maxRotation);)
+
+  CV_INIT_ALGORITHM(ICPOdometry, "RGBD.ICPOdometry",
+      obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix);
+      obj.info()->addParam(obj, "minDepth", obj.minDepth);
+      obj.info()->addParam(obj, "maxDepth", obj.maxDepth);
+      obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff);
+      obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart);
+      obj.info()->addParam(obj, "iterCounts", obj.iterCounts);
+      obj.info()->addParam(obj, "transformType", obj.transformType);
+      obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation);
+      obj.info()->addParam(obj, "maxRotation", obj.maxRotation);
+      obj.info()->addParam(obj, "normalsComputer", obj.normalsComputer, true);)
+
+  CV_INIT_ALGORITHM(RgbdICPOdometry, "RGBD.RgbdICPOdometry",
+      obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix);
+      obj.info()->addParam(obj, "minDepth", obj.minDepth);
+      obj.info()->addParam(obj, "maxDepth", obj.maxDepth);
+      obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff);
+      obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart);
+      obj.info()->addParam(obj, "iterCounts", obj.iterCounts);
+      obj.info()->addParam(obj, "minGradientMagnitudes", obj.minGradientMagnitudes);
+      obj.info()->addParam(obj, "transformType", obj.transformType);
+      obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation);
+      obj.info()->addParam(obj, "maxRotation", obj.maxRotation);
+      obj.info()->addParam(obj, "normalsComputer", obj.normalsComputer, true);)
+
+  bool
+  initModule_rgbd(void);
+  bool
+  initModule_rgbd(void)
+  {
+    bool all = true;
+    all &= !RgbdNormals_info_auto.name().empty();
+    all &= !RgbdPlane_info_auto.name().empty();
+    all &= !RgbdOdometry_info_auto.name().empty();
+    all &= !ICPOdometry_info_auto.name().empty();
+    all &= !RgbdICPOdometry_info_auto.name().empty();
+    return all;
+  }
+}

modules/rgbd/src/utils.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/rgbd.hpp>
+#include <limits>
+
+#include "utils.h"
+
+namespace cv
+{
+  /** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+   * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+   * Otherwise, the image is simply converted to floats
+   * @param in_in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+   *              (as done with the Microsoft Kinect), it is assumed in meters)
+   * @param depth the desired output depth (floats or double)
+   * @param out_out The rescaled float depth image
+   */
+  void
+  rescaleDepth(InputArray in_in, int depth, OutputArray out_out)
+  {
+    cv::Mat in = in_in.getMat();
+    CV_Assert(in.type() == CV_64FC1 || in.type() == CV_32FC1 || in.type() == CV_16UC1 || in.type() == CV_16SC1);
+    CV_Assert(depth == CV_64FC1 || depth == CV_32FC1);
+
+    int in_depth = in.depth();
+
+    out_out.create(in.size(), depth);
+    cv::Mat out = out_out.getMat();
+    if (in_depth == CV_16U)
+    {
+      in.convertTo(out, depth, 1 / 1000.0); //convert to float so that it is in meters
+      cv::Mat valid_mask = in == std::numeric_limits<uint16_t>::min(); // Should we do std::numeric_limits<uint16_t>::max() too ?
+      out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
+    }
+    if (in_depth == CV_16S)
+    {
+      in.convertTo(out, depth, 1 / 1000.0); //convert to float so tha$
+      cv::Mat valid_mask = (in == std::numeric_limits<int16_t>::min()) | (in == std::numeric_limits<int16_t>::max()); // Should we do std::numeric_limits<uint16_t>::max() too ?
+      out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
+    }
+    if ((in_depth == CV_32F) || (in_depth == CV_64F))
+      in.convertTo(out, depth);
+  }
+}

modules/rgbd/src/utils.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __OPENCV_RGBD_UTILS_HPP__
+#define __OPENCV_RGBD_UTILS_HPP__
+
+#ifdef __cplusplus
+
+#include <opencv2/rgbd.hpp>
+
+/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
+ * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
+ * Otherwise, the image is simply converted to floats
+ * @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
+ *              (as done with the Microsoft Kinect), it is assumed in meters)
+ * @param the desired output depth (floats or double)
+ * @param out The rescaled float depth image
+ */
+template<typename T>
+void
+rescaleDepthTemplated(const cv::Mat& in, cv::Mat& out);
+
+template<>
+inline void
+rescaleDepthTemplated<float>(const cv::Mat& in, cv::Mat& out)
+{
+  rescaleDepth(in, CV_32F, out);
+}
+
+template<>
+inline void
+rescaleDepthTemplated<double>(const cv::Mat& in, cv::Mat& out)
+{
+  rescaleDepth(in, CV_64F, out);
+}
+
+#endif /* __cplusplus */
+
+#endif
+
+/* End of file. */

modules/rgbd/test/test_main.cpp

+#include "test_precomp.hpp"
+
+CV_TEST_MAIN("rgbd")

modules/rgbd/test/test_normal.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <stdexcept>
+
+#include <opencv2/contrib.hpp>
+#include <opencv2/rgbd.hpp>
+
+#include "test_precomp.hpp"
+
+cv::Point3f
+rayPlaneIntersection(cv::Point2f uv, const cv::Mat& centroid, const cv::Mat& normal, const cv::Mat_<float>& Kinv);
+
+cv::Vec3f
+rayPlaneIntersection(const cv::Vec3d& uv1, double centroid_dot_normal, const cv::Vec3d& normal,
+                     const cv::Matx33d& Kinv);
+cv::Vec3f
+rayPlaneIntersection(const cv::Vec3d& uv1, double centroid_dot_normal, const cv::Vec3d& normal, const cv::Matx33d& Kinv)
+{
+
+  cv::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 d;
+  if (std::fabs(LdotNormal) > 1e-9)
+  {
+    d = centroid_dot_normal / LdotNormal;
+  }
+  else
+  {
+    d = 1.0;
+    std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl;
+    std::cout << "contents of L, Normal: " << cv::Mat(L) << ", " << cv::Mat(normal) << std::endl;
+  }
+  cv::Vec3f xyz(d * L(0), d * L(1), d * L(2));
+  return xyz;
+}
+
+cv::Point3f
+rayPlaneIntersection(cv::Point2f uv, const cv::Mat& centroid, const cv::Mat& normal, const cv::Mat_<float>& Kinv)
+{
+  cv::Matx33d dKinv(Kinv);
+  cv::Vec3d dNormal(normal);
+  return rayPlaneIntersection(cv::Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv);
+}
+
+const int W = 640;
+const int H = 480;
+int window_size = 5;
+float focal_length = 525;
+float cx = W / 2.f + 0.5f;
+float cy = H / 2.f + 0.5f;
+
+cv::Mat K = (cv::Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1);
+cv::Mat Kinv = K.inv();
+
+static cv::RNG rng;
+struct Plane
+{
+
+  cv::Vec3d 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 = n / cv::norm(n);
+    set_d(rng.uniform(-2.0, 0.6));
+  }
+
+  void
+  set_d(float d)
+  {
+    p = cv::Vec3d(0, 0, d / n[2]);
+    p_dot_n = p.dot(n);
+  }
+
+  cv::Vec3f
+  intersection(float u, float v, const cv::Matx33f& Kinv_in) const
+  {
+    return rayPlaneIntersection(cv::Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
+  }
+};
+
+void
+gen_points_3d(std::vector<Plane>& planes_out, cv::Mat_<unsigned char> &plane_mask, cv::Mat& points3d, cv::Mat& normals,
+              int n_planes);
+void
+gen_points_3d(std::vector<Plane>& planes_out, cv::Mat_<unsigned char> &plane_mask, cv::Mat& points3d, cv::Mat& normals,
+              int n_planes)
+{
+  std::vector<Plane> planes;
+  for (int i = 0; i < n_planes; i++)
+  {
+    Plane px;
+    for (int j = 0; j < 1; j++)
+    {
+      px.set_d(rng.uniform(-3.f, -0.5f));
+      planes.push_back(px);
+    }
+  }
+  cv::Mat_ < cv::Vec3f > outp(H, W);
+  cv::Mat_ < cv::Vec3f > outn(H, W);
+  plane_mask.create(H, W);
+
+  // n  ( r - r_0) = 0
+  // n * r_0 = d
+  //
+  // r_0 = (0,0,0)
+  // r[0]
+  for (int v = 0; v < H; v++)
+  {
+    for (int u = 0; u < W; u++)
+    {
+      unsigned int plane_index = (u / float(W)) * planes.size();
+      Plane plane = planes[plane_index];
+      outp(v, u) = plane.intersection(u, v, Kinv);
+      outn(v, u) = plane.n;
+      plane_mask(v, u) = plane_index;
+    }
+  }
+  planes_out = planes;
+  points3d = outp;
+  normals = outn;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class CV_RgbdNormalsTest: public cvtest::BaseTest
+{
+public:
+  CV_RgbdNormalsTest()
+  {
+  }
+  ~CV_RgbdNormalsTest()
+  {
+  }
+protected:
+  void
+  run(int)
+  {
+    try
+    {
+      cv::Mat_<unsigned char> plane_mask;
+      for (unsigned char i = 0; i < 3; ++i)
+      {
+        cv::RgbdNormals::RGBD_NORMALS_METHOD method;
+        // inner vector: whether it's 1 plane or 3 planes
+        // outer vector: float or double
+        std::vector<std::vector<float> > errors(2);
+        errors[0].resize(2);
+        errors[1].resize(2);
+        switch (i)
+        {
+          case 0:
+            method = cv::RgbdNormals::RGBD_NORMALS_METHOD_FALS;
+            std::cout << std::endl << "*** FALS" << std::endl;
+            errors[0][0] = 0.006;
+            errors[0][1] = 0.03;
+            errors[1][0] = 0.00008;
+            errors[1][1] = 0.02;
+            break;
+          case 1:
+            method = cv::RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD;
+            std::cout << std::endl << "*** LINEMOD" << std::endl;
+            errors[0][0] = 0.04;
+            errors[0][1] = 0.07;
+            errors[1][0] = 0.05;
+            errors[1][1] = 0.08;
+            break;
+          case 2:
+            method = cv::RgbdNormals::RGBD_NORMALS_METHOD_SRI;
+            std::cout << std::endl << "*** SRI" << std::endl;
+            errors[0][0] = 0.02;
+            errors[0][1] = 0.04;
+            errors[1][0] = 0.02;
+            errors[1][1] = 0.04;
+            break;
+        }
+
+        for (unsigned char j = 0; j < 2; ++j)
+        {
+          int depth = (j % 2 == 0) ? CV_32F : CV_64F;
+          if (depth == CV_32F)
+            std::cout << "* float" << std::endl;
+          else
+            std::cout << "* double" << std::endl;
+
+          cv::RgbdNormals normals_computer(H, W, depth, K, 5, method);
+          normals_computer.initialize();
+
+          std::vector<Plane> plane_params;
+          cv::Mat points3d, ground_normals;
+          // 1 plane, continuous scene, very low error..
+          std::cout << "1 plane" << std::endl;
+          float err_mean = 0;
+          for (int ii = 0; ii < 5; ++ii)
+          {
+            gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1);
+            err_mean += testit(points3d, ground_normals, normals_computer);
+          }
+          std::cout << "mean diff: " << (err_mean / 5) << std::endl;
+          EXPECT_LE(err_mean/5, errors[j][0])<< " thresh: " << errors[j][0] << std::endl;
+
+          // 3 discontinuities, more error expected.
+          std::cout << "3 planes" << std::endl;
+          err_mean = 0;
+          for (int ii = 0; ii < 5; ++ii)
+          {
+            gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3);
+            err_mean += testit(points3d, ground_normals, normals_computer);
+          }
+          std::cout << "mean diff: " << (err_mean / 5) << std::endl;
+          EXPECT_LE(err_mean/5, errors[j][1])<< "mean diff: " << (err_mean/5) << " thresh: " << errors[j][1] << std::endl;
+        }
+      }
+
+      //TODO test NaNs in data
+
+    } catch (...)
+    {
+      ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
+    }
+    ts->set_failed_test_info(cvtest::TS::OK);
+  }
+
+  float
+  testit(const cv::Mat & points3d, const cv::Mat & in_ground_normals, const cv::RgbdNormals & normals_computer)
+  {
+    cv::TickMeter tm;
+    tm.start();
+    cv::Mat in_normals;
+    if (normals_computer.method() == cv::RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
+    {
+      std::vector<cv::Mat> channels;
+      cv::split(points3d, channels);
+      normals_computer(channels[2], in_normals);
+    }
+    else
+      normals_computer(points3d, in_normals);
+    tm.stop();
+
+    cv::Mat_<cv::Vec3f> normals, ground_normals;
+    in_normals.convertTo(normals, CV_32FC3);
+    in_ground_normals.convertTo(ground_normals, CV_32FC3);
+
+    float err = 0;
+    for (int y = 0; y < normals.rows; ++y)
+      for (int x = 0; x < normals.cols; ++x)
+      {
+        cv::Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x);
+        vec1 = vec1 / cv::norm(vec1);
+        vec2 = vec2 / cv::norm(vec2);
+
+        float dot = vec1.dot(vec2);
+        // Just for rounding errors
+        if (std::abs(dot) < 1)
+          err += std::min(std::acos(dot), std::acos(-dot));
+      }
+
+    err /= normals.rows * normals.cols;
+    std::cout << "Average error: " << err << " Speed: " << tm.getTimeMilli() << " ms" << std::endl;
+    return err;
+  }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class CV_RgbdPlaneTest: public cvtest::BaseTest
+{
+public:
+  CV_RgbdPlaneTest()
+  {
+  }
+  ~CV_RgbdPlaneTest()
+  {
+  }
+protected:
+  void
+  run(int)
+  {
+    try
+    {
+      cv::RgbdPlane plane_computer;
+
+      std::vector<Plane> planes;
+      cv::Mat points3d, ground_normals;
+      cv::Mat_<unsigned char> plane_mask;
+      gen_points_3d(planes, plane_mask, points3d, ground_normals, 1);
+      testit(planes, plane_mask, points3d, plane_computer); // 1 plane, continuous scene, very low error..
+      for (int ii = 0; ii < 10; ii++)
+      {
+        gen_points_3d(planes, plane_mask, points3d, ground_normals, 3); //three planes
+        testit(planes, plane_mask, points3d, plane_computer); // 3 discontinuities, more error expected.
+      }
+    } catch (...)
+    {
+      ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
+    }
+    ts->set_failed_test_info(cvtest::TS::OK);
+  }
+
+  void
+  testit(const std::vector<Plane> & gt_planes, const cv::Mat & gt_plane_mask, const cv::Mat & points3d,
+         cv::RgbdPlane & plane_computer)
+  {
+    for (char i_test = 0; i_test < 2; ++i_test)
+    {
+      cv::TickMeter tm1, tm2;
+      cv::Mat plane_mask;
+      std::vector<cv::Vec4f> plane_coefficients;
+
+      if (i_test == 0)
+      {
+        tm1.start();
+        // First, get the normals
+        int depth = CV_32F;
+        cv::RgbdNormals normals_computer(H, W, depth, K, 5, cv::RgbdNormals::RGBD_NORMALS_METHOD_FALS);
+        cv::Mat normals;
+        normals_computer(points3d, normals);
+        tm1.stop();
+
+        tm2.start();
+        plane_computer(points3d, normals, plane_mask, plane_coefficients);
+        tm2.stop();
+      }
+      else
+      {
+        tm2.start();
+        plane_computer(points3d, plane_mask, plane_coefficients);
+        tm2.stop();
+      }
+
+      // Compare each found plane to each ground truth plane
+      size_t n_planes = plane_coefficients.size();
+      size_t n_gt_planes = gt_planes.size();
+      cv::Mat_<int> matching(n_gt_planes, n_planes);
+      for (size_t j = 0; j < n_gt_planes; ++j)
+      {
+        cv::Mat gt_mask = gt_plane_mask == j;
+        int n_gt = cv::countNonZero(gt_mask);
+        int n_max = 0, i_max = 0;
+        for (size_t i = 0; i < n_planes; ++i)
+        {
+          cv::Mat dst;
+          cv::bitwise_and(gt_mask, plane_mask == i, dst);
+          matching(j, i) = cv::countNonZero(dst);
+          if (matching(j, i) > n_max)
+          {
+            n_max = matching(j, i);
+            i_max = i;
+          }
+        }
+        // Get the best match
+        ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001);
+        // Compare the normals
+        cv::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);
+      }
+
+      std::cout << " Speed: ";
+      if (i_test == 0)
+        std::cout << "normals " << tm1.getTimeMilli() << " ms and ";
+      std::cout << "plane " << tm2.getTimeMilli() << " ms " << std::endl;
+    }
+  }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST(Rgbd_Normals, compute)
+{
+  CV_RgbdNormalsTest test;
+  test.safe_run();
+}
+
+TEST(Rgbd_Plane, compute)
+{
+  CV_RgbdPlaneTest test;
+  test.safe_run();
+}

modules/rgbd/test/test_odometry.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "test_precomp.hpp"
+
+using namespace cv;
+
+#define SHOW_DEBUG_LOG     0
+#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)
+{
+    CV_Assert(!image.empty());
+    CV_Assert(image.type() == CV_8UC1);
+    
+    CV_Assert(depth.size() == image.size());
+    CV_Assert(depth.type() == CV_32FC1);
+    
+    CV_Assert(!rvec.empty());
+    CV_Assert(rvec.total() == 3);
+    CV_Assert(rvec.type() == CV_64FC1);
+    
+    CV_Assert(!tvec.empty());
+    CV_Assert(tvec.size() == Size(1, 3));
+    CV_Assert(tvec.type() == CV_64FC1);
+    
+    warpedImage.create(image.size(), CV_8UC1);
+    warpedImage = Scalar(0);
+    warpedDepth.create(image.size(), CV_32FC1);
+    warpedDepth = Scalar(FLT_MAX);
+    
+    Mat cloud;
+    depthTo3d(depth, K, cloud);
+    Mat Rt = Mat::eye(4, 4, CV_64FC1);
+    {
+        Mat R, dst;
+        Rodrigues(rvec, R);
+        
+        dst = Rt(Rect(0,0,3,3));
+        R.copyTo(dst);
+        
+        dst = Rt(Rect(3,0,1,3));
+        tvec.copyTo(dst);
+    }
+    Mat warpedCloud, warpedImagePoints;
+    perspectiveTransform(cloud, 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);
+    for(int y = 0; y < cloud.rows; y++)
+    {
+        for(int x = 0; x < cloud.cols; x++)
+        {
+            Point p = warpedImagePoints.at<Point2f>(y,x);
+            if(r.contains(p))
+            {
+                float curDepth = warpedDepth.at<float>(p.y, p.x);
+                float newDepth = warpedCloud.at<Point3f>(y, x).z;
+                if(newDepth < curDepth && newDepth > 0)
+                {
+                    warpedImage.at<uchar>(p.y, p.x) = image.at<uchar>(y,x);
+                    warpedDepth.at<float>(p.y, p.x) = newDepth;
+                }
+            }
+        }
+    }
+    warpedDepth.setTo(std::numeric_limits<float>::quiet_NaN(), warpedDepth > 100);
+}
+
+static
+void dilateFrame(Mat& image, Mat& depth)
+{
+    CV_Assert(!image.empty());
+    CV_Assert(image.type() == CV_8UC1);
+
+    CV_Assert(!depth.empty());
+    CV_Assert(depth.type() == CV_32FC1);
+    CV_Assert(depth.size() == image.size());
+
+    Mat mask(image.size(), CV_8UC1, Scalar(255));
+    for(int y = 0; y < depth.rows; y++)
+        for(int x = 0; x < depth.cols; x++)
+            if(cvIsNaN(depth.at<float>(y,x)) || depth.at<float>(y,x) > 10 || depth.at<float>(y,x) <= FLT_EPSILON)
+                mask.at<uchar>(y,x) = 0;
+
+    image.setTo(255, ~mask);
+    Mat minImage;
+    erode(image, minImage, Mat());
+
+    image.setTo(0, ~mask);
+    Mat maxImage;
+    dilate(image, maxImage, Mat());
+
+    depth.setTo(FLT_MAX, ~mask);
+    Mat minDepth;
+    erode(depth, minDepth, Mat());
+
+    depth.setTo(0, ~mask);
+    Mat maxDepth;
+    dilate(depth, maxDepth, Mat());
+
+    Mat dilatedMask;
+    dilate(mask, dilatedMask, Mat(), Point(-1,-1), 1);
+    for(int y = 0; y < depth.rows; y++)
+        for(int x = 0; x < depth.cols; x++)
+            if(!mask.at<uchar>(y,x) && dilatedMask.at<uchar>(y,x))
+            {
+                image.at<uchar>(y,x) = static_cast<uchar>(0.5f * (static_cast<float>(minImage.at<uchar>(y,x)) +
+                                                                  static_cast<float>(maxImage.at<uchar>(y,x))));
+                depth.at<float>(y,x) = 0.5f * (minDepth.at<float>(y,x) + maxDepth.at<float>(y,x));
+            }
+}
+
+class CV_OdometryTest : public cvtest::BaseTest
+{
+public:
+    CV_OdometryTest(const Ptr<Odometry>& _odometry, 
+                    double _maxError1, 
+                    double _maxError5) :
+        odometry(_odometry), 
+        maxError1(_maxError1), 
+        maxError5(_maxError5) {}
+
+protected:
+    bool readData(Mat& image, Mat& depth) const;
+    static void generateRandomTransformation(Mat& R, Mat& t);
+    
+    virtual void run(int);
+
+    Ptr<Odometry> odometry;
+    double maxError1;
+    double maxError5;
+};
+
+bool CV_OdometryTest::readData(Mat& image, Mat& depth) const
+{
+    std::string imageFilename = std::string(ts->get_data_path()) + "/odometry/rgb.png";
+    std::string depthFilename = std::string(ts->get_data_path()) + "/odometry/depth.png";
+    
+    image = imread(imageFilename,  0);
+    depth = imread(depthFilename, -1);
+    
+    if(image.empty())
+    {
+        ts->printf( cvtest::TS::LOG, "Image %s can not be read.\n", imageFilename.c_str() );
+        ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
+        return false;
+    }
+    if(depth.empty())
+    {
+        ts->printf( cvtest::TS::LOG, "Depth %s can not be read.\n", depthFilename.c_str() );
+        ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
+        ts->set_gtest_status();
+        return false;
+    }
+    
+    CV_DbgAssert(gray.type() == CV_8UC1);
+    CV_DbgAssert(depth.type() == CV_16UC1);
+    {
+        Mat depth_flt;
+        depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
+        depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth_flt < FLT_EPSILON);
+        depth = depth_flt;
+    }
+    
+    return true;
+}
+
+void CV_OdometryTest::generateRandomTransformation(Mat& rvec, Mat& tvec)
+{
+    const float maxRotation = 3.f / 180.f * CV_PI; //rad
+    const float maxTranslation = 0.02f; //m
+
+    RNG& rng = theRNG();
+    rvec.create(3, 1, CV_64FC1);
+    tvec.create(3, 1, CV_64FC1);
+
+    randu(rvec, Scalar(-1000), Scalar(1000));
+    normalize(rvec, rvec, rng.uniform(0.007f, maxRotation));
+
+    randu(tvec, Scalar(-1000), Scalar(1000));
+    normalize(tvec, tvec, rng.uniform(0.007f, maxTranslation));
+}
+
+void CV_OdometryTest::run(int)
+{
+    float fx = 525.0f, // default
+          fy = 525.0f,
+          cx = 319.5f,
+          cy = 239.5f;
+    Mat K = Mat::eye(3,3,CV_32FC1);
+    {
+        K.at<float>(0,0) = fx;
+        K.at<float>(1,1) = fy;
+        K.at<float>(0,2) = cx;
+        K.at<float>(1,2) = cy;
+    }
+    
+    Mat image, depth;
+    if(!readData(image, depth))
+        return;
+        
+    odometry->set("cameraMatrix", K);
+    
+    Mat calcRt;
+    
+    // 1. Try to find Rt between the same frame (try masks also).
+    bool isComputed = odometry->compute(image, depth, Mat(image.size(), CV_8UC1, Scalar(255)), 
+                                        image, depth, Mat(image.size(), CV_8UC1, Scalar(255)), 
+                                        calcRt);
+    if(!isComputed)
+    {
+        ts->printf(cvtest::TS::LOG, "Can not find Rt between the same frame");
+        ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT);
+    }
+    double diff = norm(calcRt, Mat::eye(4,4,CV_64FC1));
+    if(diff > DBL_EPSILON)
+    {
+        ts->printf(cvtest::TS::LOG, "Incorrect transformation between the same frame (not the identity matrix), diff = %f", diff);
+        ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+    }
+    
+    // 2. Generate random rigid body motion in some ranges several times (iterCount). 
+    // On each iteration an input frame is warped using generated transformation.
+    // Odometry is run on the following pair: the original frame and the warped one. 
+    // Comparing a computed transformation with an applied one we compute 2 errors:
+    // better_1time_count - count of poses which error is less than ground thrush pose,
+    // better_5times_count - count of poses which error is 5 times less than ground thrush pose.
+    int iterCount = 100;
+    int better_1time_count = 0; 
+    int better_5times_count = 0;
+    for(int iter = 0; iter < iterCount; iter++)
+    {
+        Mat rvec, tvec;
+        generateRandomTransformation(rvec, tvec);
+        Mat warpedImage, warpedDepth;
+        warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth);
+        dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
+        
+        isComputed = odometry->compute(image, depth, Mat(), warpedImage, warpedDepth, Mat(), calcRt);
+        if(!isComputed)
+            continue;
+                
+        Mat calcR = calcRt(Rect(0,0,3,3)), calcRvec;
+        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
+
+    
+        // compare rotation
+        double rdiffnorm = norm(rvec - calcRvec),
+               rnorm = norm(rvec);
+        double tdiffnorm = norm(tvec - calcTvec), 
+               tnorm = norm(tvec);
+        if(rdiffnorm < rnorm &&  tdiffnorm < tnorm)
+            better_1time_count++;
+        if(5. * rdiffnorm < rnorm && 5 * tdiffnorm < tnorm)
+            better_5times_count++;
+
+#if SHOW_DEBUG_LOG
+        std::cout << "Iter " << iter << std::endl;
+        std::cout << "rdiffnorm " << rdiffnorm << "; rnorm " << rnorm << std::endl;
+        std::cout << "tdiffnorm " << tdiffnorm << "; tnorm " << tnorm << std::endl;
+        
+        std::cout << "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count << std::endl;
+#endif
+    }
+
+    
+    if(static_cast<double>(better_1time_count) < maxError1 * static_cast<double>(iterCount))
+    {
+        ts->printf(cvtest::TS::LOG, "\nIncorrect count of accurate poses [1st case]: %f / %f", static_cast<double>(better_1time_count), maxError1 * static_cast<double>(iterCount));
+        ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+    }
+    
+    if(static_cast<double>(better_5times_count) < maxError5 * static_cast<double>(iterCount))
+    {
+        ts->printf(cvtest::TS::LOG, "\nIncorrect count of accurate poses [2nd case]: %f / %f", static_cast<double>(better_5times_count), maxError5 * static_cast<double>(iterCount));
+        ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
+    }
+}
+
+/****************************************************************************************\
+*                                Tests registrations                                     *
+\****************************************************************************************/
+
+TEST(RGBD_Odometry_Rgbd, algorithmic)
+{
+    CV_OdometryTest test(Algorithm::create<Odometry>("RGBD.RgbdOdometry"), 0.99, 0.94);
+    test.safe_run();
+}
+
+TEST(RGBD_Odometry_ICP, algorithmic)
+{
+    CV_OdometryTest test(Algorithm::create<Odometry>("RGBD.ICPOdometry"), 0.99, 0.99);
+    test.safe_run();
+}
+
+TEST(RGBD_Odometry_RgbdICP, algorithmic)
+{
+    CV_OdometryTest test(Algorithm::create<Odometry>("RGBD.RgbdICPOdometry"), 0.99, 0.99);
+    test.safe_run();
+}

modules/rgbd/test/test_precomp.cpp

+#include "test_precomp.hpp"

modules/rgbd/test/test_precomp.hpp

+#ifndef __OPENCV_TEST_PRECOMP_HPP__
+#define __OPENCV_TEST_PRECOMP_HPP__
+
+#include <opencv2/ts.hpp>
+#include <opencv2/rgbd.hpp>
+#include <iostream>
+
+#endif

modules/rgbd/test/test_utils.cpp

+#include <opencv2/calib3d.hpp>
+
+#include "test_precomp.hpp"
+
+class CV_RgbdDepthTo3dTest: public cvtest::BaseTest
+{
+public:
+  CV_RgbdDepthTo3dTest()
+  {
+  }
+  ~CV_RgbdDepthTo3dTest()
+  {
+  }
+protected:
+  void
+  run(int)
+  {
+    try
+    {
+      // K from a VGA Kinect
+      cv::Mat K = (cv::Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.);
+
+      // Create a random depth image
+      cv::RNG rng;
+      cv::Mat_<float> depth(480, 640);
+      rng.fill(depth, cv::RNG::UNIFORM, 0, 100);
+
+      // Create some 3d points on the plane
+      int rows = depth.rows, cols = depth.cols;
+      cv::Mat_<cv::Vec3f> points3d;
+      cv::depthTo3d(depth, K, points3d);
+
+      // Make sure the points belong to the plane
+      cv::Mat points = points3d.reshape(1, rows*cols);
+      cv::Mat image_points;
+      cv::Mat rvec;
+      cv::Rodrigues(cv::Mat::eye(3,3,CV_32F),rvec);
+      cv::Mat tvec = (cv::Mat_<float>(1,3) << 0, 0, 0);
+      cv::projectPoints(points, rvec, tvec, K, cv::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 += cv::norm(image_points.at<cv::Vec2f>(y,x) - cv::Vec2f(x,y));
+
+      // Verify the function works
+      ASSERT_LE(avg_diff/rows/cols, 1e-4) << "Average error for ground truth is: " << (avg_diff / rows / cols);
+    } catch (...)
+    {
+      ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
+    }
+    ts->set_failed_test_info(cvtest::TS::OK);
+  }
+};
+
+TEST(Rgbd_DepthTo3d, compute)
+{
+  CV_RgbdDepthTo3dTest test;
+  test.safe_run();
+}