depth.hpp

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
 
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
 
#ifndef __OPENCV_RGBD_DEPTH_HPP__
#define __OPENCV_RGBD_DEPTH_HPP__
 
#include <opencv2/core.hpp>
#include <limits>
 
namespace cv
{
namespace rgbd
{
 
 
  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());
  }
 
  class CV_EXPORTS_W RgbdNormals: public Algorithm
  {
  public:
    enum RGBD_NORMALS_METHOD
    {
      RGBD_NORMALS_METHOD_FALS = 0,
      RGBD_NORMALS_METHOD_LINEMOD = 1,
      RGBD_NORMALS_METHOD_SRI = 2
    };
 
    RgbdNormals()
        :
          rows_(0),
          cols_(0),
          depth_(0),
          K_(Mat()),
          window_size_(0),
          method_(RGBD_NORMALS_METHOD_FALS),
          rgbd_normals_impl_(0)
    {
    }
 
    RgbdNormals(int rows, int cols, int depth, InputArray K, int window_size = 5, int method =
        RgbdNormals::RGBD_NORMALS_METHOD_FALS);
 
    ~RgbdNormals();
 
    CV_WRAP static Ptr<RgbdNormals> create(int rows, int cols, int depth, InputArray K, int window_size = 5, int method =
        RgbdNormals::RGBD_NORMALS_METHOD_FALS);
 
    CV_WRAP_AS(apply) void
    operator()(InputArray points, OutputArray normals) const;
 
    CV_WRAP void
    initialize() const;
 
    CV_WRAP int getRows() const
    {
        return rows_;
    }
    CV_WRAP void setRows(int val)
    {
        rows_ = val;
    }
    CV_WRAP int getCols() const
    {
        return cols_;
    }
    CV_WRAP void setCols(int val)
    {
        cols_ = val;
    }
    CV_WRAP int getWindowSize() const
    {
        return window_size_;
    }
    CV_WRAP void setWindowSize(int val)
    {
        window_size_ = val;
    }
    CV_WRAP int getDepth() const
    {
        return depth_;
    }
    CV_WRAP void setDepth(int val)
    {
        depth_ = val;
    }
    CV_WRAP cv::Mat getK() const
    {
        return K_;
    }
    CV_WRAP void setK(const cv::Mat &val)
    {
        K_ = val;
    }
    CV_WRAP int getMethod() const
    {
        return method_;
    }
    CV_WRAP void setMethod(int val)
    {
        method_ = val;
    }
 
  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_;
  };
 
  class CV_EXPORTS_W DepthCleaner: public Algorithm
  {
  public:
    enum DEPTH_CLEANER_METHOD
    {
      DEPTH_CLEANER_NIL
    };
 
    DepthCleaner()
        :
          depth_(0),
          window_size_(0),
          method_(DEPTH_CLEANER_NIL),
          depth_cleaner_impl_(0)
    {
    }
 
    DepthCleaner(int depth, int window_size = 5, int method = DepthCleaner::DEPTH_CLEANER_NIL);
 
    ~DepthCleaner();
 
    CV_WRAP static Ptr<DepthCleaner> create(int depth, int window_size = 5, int method = DepthCleaner::DEPTH_CLEANER_NIL);
 
    CV_WRAP_AS(apply) void
    operator()(InputArray points, OutputArray depth) const;
 
    CV_WRAP void
    initialize() const;
 
    CV_WRAP int getWindowSize() const
    {
        return window_size_;
    }
    CV_WRAP void setWindowSize(int val)
    {
        window_size_ = val;
    }
    CV_WRAP int getDepth() const
    {
        return depth_;
    }
    CV_WRAP void setDepth(int val)
    {
        depth_ = val;
    }
    CV_WRAP int getMethod() const
    {
        return method_;
    }
    CV_WRAP void setMethod(int val)
    {
        method_ = val;
    }
 
  protected:
    void
    initialize_cleaner_impl() const;
 
    int depth_;
    int window_size_;
    int method_;
    mutable void* depth_cleaner_impl_;
  };
 
 
  CV_EXPORTS_W
  void
  registerDepth(InputArray unregisteredCameraMatrix, InputArray registeredCameraMatrix, InputArray registeredDistCoeffs,
                InputArray Rt, InputArray unregisteredDepth, const Size& outputImagePlaneSize,
                OutputArray registeredDepth, bool depthDilation=false);
 
  CV_EXPORTS_W
  void
  depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d);
 
  CV_EXPORTS_W
  void
  depthTo3d(InputArray depth, InputArray K, OutputArray points3d, InputArray mask = noArray());
 
  CV_EXPORTS_W
  void
  rescaleDepth(InputArray in, int depth, OutputArray out, double depth_factor = 1000.0);
 
  class CV_EXPORTS_W RgbdPlane: public Algorithm
  {
  public:
    enum RGBD_PLANE_METHOD
    {
      RGBD_PLANE_METHOD_DEFAULT
    };
 
      RgbdPlane(int method = RgbdPlane::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)
    {
    }
 
    RgbdPlane(int method, int block_size,
              int min_size, double threshold, double sensor_error_a = 0,
              double sensor_error_b = 0, double sensor_error_c = 0);
 
    ~RgbdPlane();
 
    CV_WRAP static Ptr<RgbdPlane> create(int method, int block_size, int min_size, double threshold,
                                         double sensor_error_a = 0, double sensor_error_b = 0,
                                         double sensor_error_c = 0);
 
    CV_WRAP_AS(apply) void
    operator()(InputArray points3d, InputArray normals, OutputArray mask,
               OutputArray plane_coefficients);
 
    CV_WRAP_AS(apply) void
    operator()(InputArray points3d, OutputArray mask, OutputArray plane_coefficients);
 
    CV_WRAP int getBlockSize() const
    {
        return block_size_;
    }
    CV_WRAP void setBlockSize(int val)
    {
        block_size_ = val;
    }
    CV_WRAP int getMinSize() const
    {
        return min_size_;
    }
    CV_WRAP void setMinSize(int val)
    {
        min_size_ = val;
    }
    CV_WRAP int getMethod() const
    {
        return method_;
    }
    CV_WRAP void setMethod(int val)
    {
        method_ = val;
    }
    CV_WRAP double getThreshold() const
    {
        return threshold_;
    }
    CV_WRAP void setThreshold(double val)
    {
        threshold_ = val;
    }
    CV_WRAP double getSensorErrorA() const
    {
        return sensor_error_a_;
    }
    CV_WRAP void setSensorErrorA(double val)
    {
        sensor_error_a_ = val;
    }
    CV_WRAP double getSensorErrorB() const
    {
        return sensor_error_b_;
    }
    CV_WRAP void setSensorErrorB(double val)
    {
        sensor_error_b_ = val;
    }
    CV_WRAP double getSensorErrorC() const
    {
        return sensor_error_c_;
    }
    CV_WRAP void setSensorErrorC(double val)
    {
        sensor_error_c_ = val;
    }
 
  private:
    int method_;
    int block_size_;
    int min_size_;
    double threshold_;
    double sensor_error_a_, sensor_error_b_, sensor_error_c_;
  };
 
  struct CV_EXPORTS_W RgbdFrame
  {
      RgbdFrame();
      RgbdFrame(const Mat& image, const Mat& depth, const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1);
      virtual ~RgbdFrame();
 
      CV_WRAP static Ptr<RgbdFrame> create(const Mat& image=Mat(), const Mat& depth=Mat(), const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1);
 
      CV_WRAP virtual void
      release();
 
      CV_PROP int ID;
      CV_PROP Mat image;
      CV_PROP Mat depth;
      CV_PROP Mat mask;
      CV_PROP Mat normals;
  };
 
  struct CV_EXPORTS_W OdometryFrame : public RgbdFrame
  {
    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);
 
    CV_WRAP static Ptr<OdometryFrame> create(const Mat& image=Mat(), const Mat& depth=Mat(), const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1);
 
    CV_WRAP virtual void
    release() CV_OVERRIDE;
 
    CV_WRAP void
    releasePyramids();
 
    CV_PROP std::vector<Mat> pyramidImage;
    CV_PROP std::vector<Mat> pyramidDepth;
    CV_PROP std::vector<Mat> pyramidMask;
 
    CV_PROP std::vector<Mat> pyramidCloud;
 
    CV_PROP std::vector<Mat> pyramid_dI_dx;
    CV_PROP std::vector<Mat> pyramid_dI_dy;
    CV_PROP std::vector<Mat> pyramidTexturedMask;
 
    CV_PROP std::vector<Mat> pyramidNormals;
    CV_PROP std::vector<Mat> pyramidNormalsMask;
  };
 
  class CV_EXPORTS_W Odometry: public Algorithm
  {
  public:
 
    enum
    {
      ROTATION = 1, TRANSLATION = 2, RIGID_BODY_MOTION = 4
    };
 
    CV_WRAP static inline float
    DEFAULT_MIN_DEPTH()
    {
      return 0.f; // in meters
    }
    CV_WRAP static inline float
    DEFAULT_MAX_DEPTH()
    {
      return 4.f; // in meters
    }
    CV_WRAP static inline float
    DEFAULT_MAX_DEPTH_DIFF()
    {
      return 0.07f; // in meters
    }
    CV_WRAP static inline float
    DEFAULT_MAX_POINTS_PART()
    {
      return 0.07f; // in [0, 1]
    }
    CV_WRAP static inline float
    DEFAULT_MAX_TRANSLATION()
    {
      return 0.15f; // in meters
    }
    CV_WRAP static inline float
    DEFAULT_MAX_ROTATION()
    {
      return 15; // in degrees
    }
 
    CV_WRAP bool
    compute(const Mat& srcImage, const Mat& srcDepth, const Mat& srcMask, const Mat& dstImage, const Mat& dstDepth,
            const Mat& dstMask, OutputArray Rt, const Mat& initRt = Mat()) const;
 
    CV_WRAP_AS(compute2) bool
    compute(Ptr<OdometryFrame>& srcFrame, Ptr<OdometryFrame>& dstFrame, OutputArray Rt, const Mat& initRt = Mat()) const;
 
    CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const;
 
    CV_WRAP static Ptr<Odometry> create(const String & odometryType);
 
    CV_WRAP virtual cv::Mat getCameraMatrix() const = 0;
    CV_WRAP virtual void setCameraMatrix(const cv::Mat &val) = 0;
    CV_WRAP virtual int getTransformType() const = 0;
    CV_WRAP virtual void setTransformType(int val) = 0;
 
  protected:
    virtual void
    checkParams() const = 0;
 
    virtual bool
    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
                const Mat& initRt) const = 0;
  };
 
  class CV_EXPORTS_W RgbdOdometry: public Odometry
  {
  public:
    RgbdOdometry();
    RgbdOdometry(const Mat& cameraMatrix, float minDepth = Odometry::DEFAULT_MIN_DEPTH(), float maxDepth = Odometry::DEFAULT_MAX_DEPTH(),
                 float maxDepthDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(), const std::vector<int>& iterCounts = std::vector<int>(),
                 const std::vector<float>& minGradientMagnitudes = std::vector<float>(), float maxPointsPart = Odometry::DEFAULT_MAX_POINTS_PART(),
                 int transformType = Odometry::RIGID_BODY_MOTION);
 
    CV_WRAP static Ptr<RgbdOdometry> create(const Mat& cameraMatrix = Mat(), float minDepth = Odometry::DEFAULT_MIN_DEPTH(), float maxDepth = Odometry::DEFAULT_MAX_DEPTH(),
                 float maxDepthDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(), const std::vector<int>& iterCounts = std::vector<int>(),
                 const std::vector<float>& minGradientMagnitudes = std::vector<float>(), float maxPointsPart = Odometry::DEFAULT_MAX_POINTS_PART(),
                 int transformType = Odometry::RIGID_BODY_MOTION);
 
    CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const CV_OVERRIDE;
 
    CV_WRAP cv::Mat getCameraMatrix() const CV_OVERRIDE
    {
        return cameraMatrix;
    }
    CV_WRAP void setCameraMatrix(const cv::Mat &val) CV_OVERRIDE
    {
        cameraMatrix = val;
    }
    CV_WRAP double getMinDepth() const
    {
        return minDepth;
    }
    CV_WRAP void setMinDepth(double val)
    {
        minDepth = val;
    }
    CV_WRAP double getMaxDepth() const
    {
        return maxDepth;
    }
    CV_WRAP void setMaxDepth(double val)
    {
        maxDepth = val;
    }
    CV_WRAP double getMaxDepthDiff() const
    {
        return maxDepthDiff;
    }
    CV_WRAP void setMaxDepthDiff(double val)
    {
        maxDepthDiff = val;
    }
    CV_WRAP cv::Mat getIterationCounts() const
    {
        return iterCounts;
    }
    CV_WRAP void setIterationCounts(const cv::Mat &val)
    {
        iterCounts = val;
    }
    CV_WRAP cv::Mat getMinGradientMagnitudes() const
    {
        return minGradientMagnitudes;
    }
    CV_WRAP void setMinGradientMagnitudes(const cv::Mat &val)
    {
        minGradientMagnitudes = val;
    }
    CV_WRAP double getMaxPointsPart() const
    {
        return maxPointsPart;
    }
    CV_WRAP void setMaxPointsPart(double val)
    {
        maxPointsPart = val;
    }
    CV_WRAP int getTransformType() const CV_OVERRIDE
    {
        return transformType;
    }
    CV_WRAP void setTransformType(int val) CV_OVERRIDE
    {
        transformType = val;
    }
    CV_WRAP double getMaxTranslation() const
    {
        return maxTranslation;
    }
    CV_WRAP void setMaxTranslation(double val)
    {
        maxTranslation = val;
    }
    CV_WRAP double getMaxRotation() const
    {
        return maxRotation;
    }
    CV_WRAP void setMaxRotation(double val)
    {
        maxRotation = val;
    }
 
  protected:
    virtual void
    checkParams() const CV_OVERRIDE;
 
    virtual bool
    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
                const Mat& initRt) const CV_OVERRIDE;
 
    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
    /*float*/
    double minDepth, maxDepth, maxDepthDiff;
    /*vector<int>*/
    Mat iterCounts;
    /*vector<float>*/
    Mat minGradientMagnitudes;
    double maxPointsPart;
 
    Mat cameraMatrix;
    int transformType;
 
    double maxTranslation, maxRotation;
  };
 
  class CV_EXPORTS_W ICPOdometry: public Odometry
  {
  public:
    ICPOdometry();
    ICPOdometry(const Mat& cameraMatrix, float minDepth = Odometry::DEFAULT_MIN_DEPTH(), float maxDepth = Odometry::DEFAULT_MAX_DEPTH(),
                float maxDepthDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = Odometry::DEFAULT_MAX_POINTS_PART(),
                const std::vector<int>& iterCounts = std::vector<int>(), int transformType = Odometry::RIGID_BODY_MOTION);
 
    CV_WRAP static Ptr<ICPOdometry> create(const Mat& cameraMatrix = Mat(), float minDepth = Odometry::DEFAULT_MIN_DEPTH(), float maxDepth = Odometry::DEFAULT_MAX_DEPTH(),
                float maxDepthDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = Odometry::DEFAULT_MAX_POINTS_PART(),
                const std::vector<int>& iterCounts = std::vector<int>(), int transformType = Odometry::RIGID_BODY_MOTION);
 
    CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const CV_OVERRIDE;
 
    CV_WRAP cv::Mat getCameraMatrix() const CV_OVERRIDE
    {
        return cameraMatrix;
    }
    CV_WRAP void setCameraMatrix(const cv::Mat &val) CV_OVERRIDE
    {
        cameraMatrix = val;
    }
    CV_WRAP double getMinDepth() const
    {
        return minDepth;
    }
    CV_WRAP void setMinDepth(double val)
    {
        minDepth = val;
    }
    CV_WRAP double getMaxDepth() const
    {
        return maxDepth;
    }
    CV_WRAP void setMaxDepth(double val)
    {
        maxDepth = val;
    }
    CV_WRAP double getMaxDepthDiff() const
    {
        return maxDepthDiff;
    }
    CV_WRAP void setMaxDepthDiff(double val)
    {
        maxDepthDiff = val;
    }
    CV_WRAP cv::Mat getIterationCounts() const
    {
        return iterCounts;
    }
    CV_WRAP void setIterationCounts(const cv::Mat &val)
    {
        iterCounts = val;
    }
    CV_WRAP double getMaxPointsPart() const
    {
        return maxPointsPart;
    }
    CV_WRAP void setMaxPointsPart(double val)
    {
        maxPointsPart = val;
    }
    CV_WRAP int getTransformType() const CV_OVERRIDE
    {
        return transformType;
    }
    CV_WRAP void setTransformType(int val) CV_OVERRIDE
    {
        transformType = val;
    }
    CV_WRAP double getMaxTranslation() const
    {
        return maxTranslation;
    }
    CV_WRAP void setMaxTranslation(double val)
    {
        maxTranslation = val;
    }
    CV_WRAP double getMaxRotation() const
    {
        return maxRotation;
    }
    CV_WRAP void setMaxRotation(double val)
    {
        maxRotation = val;
    }
    CV_WRAP Ptr<RgbdNormals> getNormalsComputer() const
    {
        return normalsComputer;
    }
 
  protected:
    virtual void
    checkParams() const CV_OVERRIDE;
 
    virtual bool
    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
                const Mat& initRt) const CV_OVERRIDE;
 
    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
    /*float*/
    double minDepth, maxDepth, maxDepthDiff;
    /*float*/
    double maxPointsPart;
    /*vector<int>*/
    Mat iterCounts;
 
    Mat cameraMatrix;
    int transformType;
 
    double maxTranslation, maxRotation;
 
    mutable Ptr<RgbdNormals> normalsComputer;
  };
 
  class CV_EXPORTS_W RgbdICPOdometry: public Odometry
  {
  public:
    RgbdICPOdometry();
    RgbdICPOdometry(const Mat& cameraMatrix, float minDepth = Odometry::DEFAULT_MIN_DEPTH(), float maxDepth = Odometry::DEFAULT_MAX_DEPTH(),
                    float maxDepthDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = Odometry::DEFAULT_MAX_POINTS_PART(),
                    const std::vector<int>& iterCounts = std::vector<int>(),
                    const std::vector<float>& minGradientMagnitudes = std::vector<float>(),
                    int transformType = Odometry::RIGID_BODY_MOTION);
 
    CV_WRAP static Ptr<RgbdICPOdometry> create(const Mat& cameraMatrix = Mat(), float minDepth = Odometry::DEFAULT_MIN_DEPTH(), float maxDepth = Odometry::DEFAULT_MAX_DEPTH(),
                    float maxDepthDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = Odometry::DEFAULT_MAX_POINTS_PART(),
                    const std::vector<int>& iterCounts = std::vector<int>(),
                    const std::vector<float>& minGradientMagnitudes = std::vector<float>(),
                    int transformType = Odometry::RIGID_BODY_MOTION);
 
    CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const CV_OVERRIDE;
 
    CV_WRAP cv::Mat getCameraMatrix() const CV_OVERRIDE
    {
        return cameraMatrix;
    }
    CV_WRAP void setCameraMatrix(const cv::Mat &val) CV_OVERRIDE
    {
        cameraMatrix = val;
    }
    CV_WRAP double getMinDepth() const
    {
        return minDepth;
    }
    CV_WRAP void setMinDepth(double val)
    {
        minDepth = val;
    }
    CV_WRAP double getMaxDepth() const
    {
        return maxDepth;
    }
    CV_WRAP void setMaxDepth(double val)
    {
        maxDepth = val;
    }
    CV_WRAP double getMaxDepthDiff() const
    {
        return maxDepthDiff;
    }
    CV_WRAP void setMaxDepthDiff(double val)
    {
        maxDepthDiff = val;
    }
    CV_WRAP double getMaxPointsPart() const
    {
        return maxPointsPart;
    }
    CV_WRAP void setMaxPointsPart(double val)
    {
        maxPointsPart = val;
    }
    CV_WRAP cv::Mat getIterationCounts() const
    {
        return iterCounts;
    }
    CV_WRAP void setIterationCounts(const cv::Mat &val)
    {
        iterCounts = val;
    }
    CV_WRAP cv::Mat getMinGradientMagnitudes() const
    {
        return minGradientMagnitudes;
    }
    CV_WRAP void setMinGradientMagnitudes(const cv::Mat &val)
    {
        minGradientMagnitudes = val;
    }
    CV_WRAP int getTransformType() const CV_OVERRIDE
    {
        return transformType;
    }
    CV_WRAP void setTransformType(int val) CV_OVERRIDE
    {
        transformType = val;
    }
    CV_WRAP double getMaxTranslation() const
    {
        return maxTranslation;
    }
    CV_WRAP void setMaxTranslation(double val)
    {
        maxTranslation = val;
    }
    CV_WRAP double getMaxRotation() const
    {
        return maxRotation;
    }
    CV_WRAP void setMaxRotation(double val)
    {
        maxRotation = val;
    }
    CV_WRAP Ptr<RgbdNormals> getNormalsComputer() const
    {
        return normalsComputer;
    }
 
  protected:
    virtual void
    checkParams() const CV_OVERRIDE;
 
    virtual bool
    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
                const Mat& initRt) const CV_OVERRIDE;
 
    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
    /*float*/
    double minDepth, maxDepth, maxDepthDiff;
    /*float*/
    double maxPointsPart;
    /*vector<int>*/
    Mat iterCounts;
    /*vector<float>*/
    Mat minGradientMagnitudes;
 
    Mat cameraMatrix;
    int transformType;
 
    double maxTranslation, maxRotation;
 
    mutable Ptr<RgbdNormals> normalsComputer;
  };
 
  class CV_EXPORTS_W FastICPOdometry: public Odometry
  {
  public:
    FastICPOdometry();
    FastICPOdometry(const Mat& cameraMatrix,
                    float maxDistDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(),
                    float angleThreshold = (float)(30. * CV_PI / 180.),
                    float sigmaDepth = 0.04f,
                    float sigmaSpatial = 4.5f,
                    int kernelSize = 7,
                    const std::vector<int>& iterCounts = std::vector<int>());
 
    CV_WRAP static Ptr<FastICPOdometry> create(const Mat& cameraMatrix,
                                               float maxDistDiff = Odometry::DEFAULT_MAX_DEPTH_DIFF(),
                                               float angleThreshold = (float)(30. * CV_PI / 180.),
                                               float sigmaDepth = 0.04f,
                                               float sigmaSpatial = 4.5f,
                                               int kernelSize = 7,
                                               const std::vector<int>& iterCounts = std::vector<int>());
 
    CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const CV_OVERRIDE;
 
    CV_WRAP cv::Mat getCameraMatrix() const CV_OVERRIDE
    {
        return cameraMatrix;
    }
    CV_WRAP void setCameraMatrix(const cv::Mat &val) CV_OVERRIDE
    {
        cameraMatrix = val;
    }
    CV_WRAP double getMaxDistDiff() const
    {
        return maxDistDiff;
    }
    CV_WRAP void setMaxDistDiff(float val)
    {
        maxDistDiff = val;
    }
    CV_WRAP float getAngleThreshold() const
    {
        return angleThreshold;
    }
    CV_WRAP void setAngleThreshold(float f)
    {
        angleThreshold = f;
    }
    CV_WRAP float getSigmaDepth() const
    {
        return sigmaDepth;
    }
    CV_WRAP void setSigmaDepth(float f)
    {
        sigmaDepth = f;
    }
    CV_WRAP float getSigmaSpatial() const
    {
        return sigmaSpatial;
    }
    CV_WRAP void setSigmaSpatial(float f)
    {
        sigmaSpatial = f;
    }
    CV_WRAP int getKernelSize() const
    {
        return kernelSize;
    }
    CV_WRAP void setKernelSize(int f)
    {
        kernelSize = f;
    }
    CV_WRAP cv::Mat getIterationCounts() const
    {
        return iterCounts;
    }
    CV_WRAP void setIterationCounts(const cv::Mat &val)
    {
        iterCounts = val;
    }
    CV_WRAP int getTransformType() const CV_OVERRIDE
    {
        return Odometry::RIGID_BODY_MOTION;
    }
    CV_WRAP void setTransformType(int val) CV_OVERRIDE
    {
        if(val != Odometry::RIGID_BODY_MOTION)
            throw std::runtime_error("Rigid Body Motion is the only accepted transformation type"
                                     " for this odometry method");
    }
 
  protected:
    virtual void
    checkParams() const CV_OVERRIDE;
 
    virtual bool
    computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, OutputArray Rt,
                const Mat& initRt) const CV_OVERRIDE;
 
    // Some params have commented desired type. It's due to AlgorithmInfo::addParams does not support it now.
    float maxDistDiff;
 
    float angleThreshold;
 
    float sigmaDepth;
 
    float sigmaSpatial;
 
    int kernelSize;
 
    /*vector<int>*/
    Mat iterCounts;
 
    Mat cameraMatrix;
  };
 
  CV_EXPORTS_W
  void
  warpFrame(const Mat& image, const Mat& depth, const Mat& mask, const Mat& Rt, const Mat& cameraMatrix,
            const Mat& distCoeff, OutputArray warpedImage, OutputArray warpedDepth = noArray(), OutputArray warpedMask = noArray());
 
// TODO Depth interpolation
// Curvature
// Get rescaleDepth return dubles if asked for
 
 
} /* namespace rgbd */
} /* namespace cv */
 
#endif
 
/* End of file. */