diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt new file mode 100644 index 000000000..165bd4626 --- /dev/null +++ b/modules/rgbd/CMakeLists.txt @@ -0,0 +1,3 @@ +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) diff --git a/modules/rgbd/doc/rgbd.rst b/modules/rgbd/doc/rgbd.rst new file mode 100644 index 000000000..ea586449e --- /dev/null +++ b/modules/rgbd/doc/rgbd.rst @@ -0,0 +1,12 @@ +************************** +rgbd. RGB-Depth Processing +************************** + +.. highlight:: cpp + +.. toctree:: + :maxdepth: 2 + + depth_image + geometry + odometry diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp new file mode 100644 index 000000000..2ebdef851 --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd.hpp @@ -0,0 +1,658 @@ +/* + * 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 + +#include + +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::min()) && (depth != std::numeric_limits::max()); + } + CV_EXPORTS + inline bool + isValidDepth(const unsigned short int & depth) + { + return (depth != std::numeric_limits::min()) + && (depth != std::numeric_limits::max()); + } + CV_EXPORTS + inline bool + isValidDepth(const int & depth) + { + return (depth != std::numeric_limits::min()) && (depth != std::numeric_limits::max()); + } + CV_EXPORTS + inline bool + isValidDepth(const unsigned int & depth) + { + return (depth != std::numeric_limits::min()) && (depth != std::numeric_limits::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::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 pyramidImage; + std::vector pyramidDepth; + std::vector pyramidMask; + + std::vector pyramidCloud; + + std::vector pyramid_dI_dx; + std::vector pyramid_dI_dy; + std::vector pyramidTexturedMask; + + std::vector pyramidNormals; + std::vector 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& srcFrame, Ptr& 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& frame, int cacheType) const; + + protected: + virtual void + checkParams() const = 0; + + virtual bool + computeImpl(const Ptr& srcFrame, const Ptr& 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& iterCounts = std::vector(), + const std::vector& minGradientMagnitudes = std::vector(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(), + int transformType = RIGID_BODY_MOTION); + + virtual Size + prepareFrameCache(Ptr& frame, int cacheType) const; + + AlgorithmInfo* + info() const; + + protected: + virtual void + checkParams() const; + + virtual bool + computeImpl(const Ptr& srcFrame, const Ptr& 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*/ + Mat iterCounts; + /*vector*/ + 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& iterCounts = std::vector(), int transformType = RIGID_BODY_MOTION); + + virtual Size + prepareFrameCache(Ptr& frame, int cacheType) const; + + AlgorithmInfo* + info() const; + + protected: + virtual void + checkParams() const; + + virtual bool + computeImpl(const Ptr& srcFrame, const Ptr& 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*/ + Mat iterCounts; + + Mat cameraMatrix; + int transformType; + + double maxTranslation, maxRotation; + + mutable cv::Ptr 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& iterCounts = std::vector(), + const std::vector& minGradientMagnitudes = std::vector(), + int transformType = RIGID_BODY_MOTION); + + virtual Size + prepareFrameCache(Ptr& frame, int cacheType) const; + + AlgorithmInfo* + info() const; + + protected: + virtual void + checkParams() const; + + virtual bool + computeImpl(const Ptr& srcFrame, const Ptr& 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*/ + Mat iterCounts; + /*vector*/ + Mat minGradientMagnitudes; + + Mat cameraMatrix; + int transformType; + + double maxTranslation, maxRotation; + + mutable cv::Ptr 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. */ diff --git a/modules/rgbd/samples/CMakeLists.txt b/modules/rgbd/samples/CMakeLists.txt new file mode 100644 index 000000000..1f7390679 --- /dev/null +++ b/modules/rgbd/samples/CMakeLists.txt @@ -0,0 +1,9 @@ +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}) diff --git a/modules/rgbd/samples/odometry_evaluation.cpp b/modules/rgbd/samples/odometry_evaluation.cpp new file mode 100644 index 000000000..fdad37132 --- /dev/null +++ b/modules/rgbd/samples/odometry_evaluation.cpp @@ -0,0 +1,250 @@ +/* + * 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 + +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/contrib/contrib.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +#include +#include +#include + +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& timestamps, const vector& 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(0,3) << " " << Rt_curr.at(1,3) << " " << Rt_curr.at(2,3) << " " + << rvec.at(0) << " " << rvec.at(1) << " " << rvec.at(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 timestamps; + vector 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(0,0) = fx; + cameraMatrix.at(1,1) = fy; + cameraMatrix.at(0,2) = cx; + cameraMatrix.at(1,2) = cy; + } + + Ptr frame_prev = new OdometryFrame(), + frame_curr = new OdometryFrame(); + Ptr odometry = Algorithm::create("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::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::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; +} diff --git a/modules/rgbd/src/depth_cleaner.cpp b/modules/rgbd/src/depth_cleaner.cpp new file mode 100644 index 000000000..d99da3c9e --- /dev/null +++ b/modules/rgbd/src/depth_cleaner.cpp @@ -0,0 +1,326 @@ +/* + * 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 +#include +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +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 + class NIL: public DepthCleanerImpl + { + public: + typedef cv::Vec Vec3T; + typedef cv::Matx 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_ &depth(depth_in); + cv::Mat depth_out_tmp; + computeImpl(depth, depth_out_tmp, 0.001); + depth_out_tmp.convertTo(depth_out, CV_16U); + break; + } + case CV_32F: + { + const cv::Mat_ &depth(depth_in); + computeImpl(depth, depth_out, 1); + break; + } + case CV_64F: + { + const cv::Mat_ &depth(depth_in); + computeImpl(depth, depth_out, 1); + break; + } + } + } + + private: + /** Compute the normals + * @param r + * @return + */ + template + void + computeImpl(const cv::Mat_ &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_ 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_ Dw_sum = cv::Mat_::zeros(rows, cols), w_sum = + cv::Mat_::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 *>(depth_cleaner_impl_); + break; + case CV_32F: + delete reinterpret_cast *>(depth_cleaner_impl_); + break; + case CV_64F: + delete reinterpret_cast *>(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(window_size_, depth_, DEPTH_CLEANER_NIL); + break; + case CV_32F: + depth_cleaner_impl_ = new NIL(window_size_, depth_, DEPTH_CLEANER_NIL); + break; + case CV_64F: + depth_cleaner_impl_ = new NIL(window_size_, depth_, DEPTH_CLEANER_NIL); + break; + } + break; + } + } + + reinterpret_cast(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(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 *>(depth_cleaner_impl_)->compute(depth_in, depth_out); + break; + case CV_32F: + reinterpret_cast *>(depth_cleaner_impl_)->compute(depth_in, depth_out); + break; + case CV_64F: + reinterpret_cast *>(depth_cleaner_impl_)->compute(depth_in, depth_out); + break; + } + break; + } + } + } +} diff --git a/modules/rgbd/src/depth_to_3d.cpp b/modules/rgbd/src/depth_to_3d.cpp new file mode 100644 index 000000000..d3411f293 --- /dev/null +++ b/modules/rgbd/src/depth_to_3d.cpp @@ -0,0 +1,265 @@ +/* + * 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 +#include + +#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_ 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 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_ u_mat, v_mat, z_mat; + + cv::Mat_ 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(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat); + else if (depth.depth() == CV_16S) + n_points = convertDepthToFloat(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat); + else + { + CV_Assert(depth.type() == CV_32F); + n_points = convertDepthToFloat(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 + void + depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_& 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_ z_mat; + if (z_mat.depth() == in_depth.depth()) + z_mat = in_depth; + else + rescaleDepthTemplated(in_depth, z_mat); + + // Pre-compute some constants + cv::Mat_ 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* point = points3d.ptr >(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_ z_mat; + + if (depth.depth() == CV_16U) + convertDepthToFloat(depth, 1.0 / 1000.0f, points_float, z_mat); + else if (depth.depth() == CV_16U) + convertDepthToFloat(depth, 1.0 / 1000.0f, points_float, z_mat); + else + { + CV_Assert(depth.type() == CV_32F); + convertDepthToFloat(depth, 1.0f, points_float, z_mat); + } + + std::vector 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(depth, K_new, points3d); + else + depthTo3dNoMask(depth, K_new, points3d); + } + } +} diff --git a/modules/rgbd/src/depth_to_3d.h b/modules/rgbd/src/depth_to_3d.h new file mode 100644 index 000000000..dcbd09d9f --- /dev/null +++ b/modules/rgbd/src/depth_to_3d.h @@ -0,0 +1,121 @@ +/* + * 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 +#include + +/** + * @param depth the depth image, containing depth with the value T + * @param the mask, containing CV_8UC1 + */ +template +size_t +convertDepthToFloat(const cv::Mat& depth, const cv::Mat& mask, float scale, cv::Mat_ &u_mat, cv::Mat_ &v_mat, cv::Mat_ &z_mat) +{ + CV_Assert (depth.size == mask.size); + + cv::Size depth_size = depth.size(); + + cv::Mat_ uchar_mask = mask; + + if (mask.depth() != CV_8U) + mask.convertTo(uchar_mask, CV_8U); + + u_mat = cv::Mat_(depth_size.area(), 1); + v_mat = cv::Mat_(depth_size.area(), 1); + z_mat = cv::Mat_(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(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(v, u); + + if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits::min()) || (depth_i == std::numeric_limits::max())) + z_mat(n_points, 0) = std::numeric_limits::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 +void +convertDepthToFloat(const cv::Mat& depth, float scale, const cv::Mat &uv_mat, cv::Mat_ &z_mat) +{ + z_mat = cv::Mat_(uv_mat.size()); + + // Raw data from the Kinect has int + float* z_mat_iter = reinterpret_cast(z_mat.data); + + for (cv::Mat_::const_iterator uv_iter = uv_mat.begin(), uv_end = uv_mat.end(); + 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::quiet_NaN(); + else + *z_mat_iter = depth_i * scale; + } +} + +#endif /* __cplusplus */ + +#endif + +/* End of file. */ diff --git a/modules/rgbd/src/normal.cpp b/modules/rgbd/src/normal.cpp new file mode 100644 index 000000000..c0000328f --- /dev/null +++ b/modules/rgbd/src/normal.cpp @@ -0,0 +1,888 @@ +/* + * 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 +#include +#include +#include + +namespace +{ + /** Just compute the norm of a vector + * @param vec a vector of size 3 and any type T + * @return + */ + template + T + inline + norm_vec(const cv::Vec &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 + cv::Mat_ + computeRadius(const cv::Mat &points) + { + typedef cv::Vec PointT; + + // Compute the + cv::Size size(points.cols, points.rows); + cv::Mat_ 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 + void + computeThetaPhi(int rows, int cols, const cv::Matx& 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 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(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 + inline + void + signNormal(const cv::Vec & normal_in, cv::Vec & normal_out) + { + cv::Vec 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 + inline + void + signNormal(T a, T b, T c, cv::Vec & 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 + class FALS: public RgbdNormalsImpl + { + public: + typedef cv::Matx Mat33T; + typedef cv::Vec Vec9T; + typedef cv::Vec 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(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi); + + // Compute all the v_i for every points + std::vector 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_ 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_ 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(M_inv_.ptr(0)); + Vec3T *normal = normals.ptr(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_ V_; + cv::Mat_ 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 +void +multiply_by_K_inv(const cv::Matx & K_inv, U a, U b, U c, cv::Vec &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 + class LINEMOD: public RgbdNormalsImpl + { + public: + typedef cv::Vec Vec3T; + typedef cv::Matx 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_ &depth(depth_in); + computeImpl(depth, normals); + break; + } + case CV_32F: + { + const cv::Mat_ &depth(depth_in); + computeImpl(depth, normals); + break; + } + case CV_64F: + { + const cv::Mat_ &depth(depth_in); + computeImpl(depth, normals); + break; + } + } + } + + private: + /** Compute the normals + * @param r + * @return + */ + template + cv::Mat + computeImpl(const cv::Mat_ &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::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(depth.ptr(y, r)); + Vec3T *normal = normals.ptr(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 + class SRI: public RgbdNormalsImpl + { + public: + typedef cv::Matx Mat33T; + typedef cv::Vec Vec9T; + typedef cv::Vec 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_ cos_theta, sin_theta, cos_phi, sin_phi; + computeThetaPhi(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 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_ 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(0,0), cx = K_.at(0,2); + double invFy = 1.0f/K_.at(1,1), cy = K_.at(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_& r_T(r); + const cv::Mat_ &points3d_T(points3d); + compute(points3d_T, r_T, normals); + } + + /** Compute the normals + * @param r + * @return + */ + void + compute(const cv::Mat_ &, const cv::Mat_ &r_non_interp, cv::Mat & normals_out) const + { + // Interpolate the radial image to make derivatives meaningful + cv::Mat_ 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_ 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_ 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(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(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_ 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_ map_; + cv::Mat xy_, fxy_; + + cv::Mat_ 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 *>(rgbd_normals_impl_); + else + delete reinterpret_cast *>(rgbd_normals_impl_); + break; + } + case RgbdNormals::RGBD_NORMALS_METHOD_SRI: + { + if (depth == CV_32F) + delete reinterpret_cast *>(rgbd_normals_impl_); + else + delete reinterpret_cast *>(rgbd_normals_impl_); + break; + } + case (RgbdNormals::RGBD_NORMALS_METHOD_FALS): + { + if (depth == CV_32F) + delete reinterpret_cast *>(rgbd_normals_impl_); + else + delete reinterpret_cast *>(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(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_FALS); + else + rgbd_normals_impl_ = new FALS(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(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_LINEMOD); + else + rgbd_normals_impl_ = new LINEMOD(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(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_SRI); + else + rgbd_normals_impl_ = new SRI(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_SRI); + break; + } + } + + reinterpret_cast(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(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(points3d); + else + radius = computeRadius(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 *>(rgbd_normals_impl_)->compute(points3d, radius, normals); + else + reinterpret_cast *>(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 channels; + cv::split(points3d, channels); + depth = channels[2]; + } + else + depth = points3d_ori; + + if (depth_ == CV_32F) + reinterpret_cast *>(rgbd_normals_impl_)->compute(depth, normals); + else + reinterpret_cast *>(rgbd_normals_impl_)->compute(depth, normals); + break; + } + case RGBD_NORMALS_METHOD_SRI: + { + if (depth_ == CV_32F) + reinterpret_cast *>(rgbd_normals_impl_)->compute(points3d, radius, normals); + else + reinterpret_cast *>(rgbd_normals_impl_)->compute(points3d, radius, normals); + break; + } + } + } +} diff --git a/modules/rgbd/src/odometry.cpp b/modules/rgbd/src/odometry.cpp new file mode 100644 index 000000000..ee292cea1 --- /dev/null +++ b/modules/rgbd/src/odometry.cpp @@ -0,0 +1,1406 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3 +#define HAVE_EIGEN3_HERE +#include +#include +#include +#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& 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(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& 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& 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& pyramidDepth, float minDepth, float maxDepth, + const std::vector& pyramidNormal, + std::vector& 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 channelMasks; + split(validNormalMask, channelMasks); + validNormalMask = channelMasks[0] & channelMasks[1] & channelMasks[2]; + + levelMask &= validNormalMask; + } + } + } +} + +static +void preparePyramidCloud(const std::vector& pyramidDepth, const Mat& cameraMatrix, std::vector& 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 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& pyramidImage, int dx, int dy, std::vector& 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(y,x)) + { + subset.at(y,x) = 255; + mask.at(y,x) = 0; + subsetSize++; + } + } + mask = subset; + } +} + +static +void preparePyramidTexturedMask(const std::vector& pyramid_dI_dx, const std::vector& pyramid_dI_dy, + const std::vector& minGradMagnitudes, const std::vector& pyramidMask, double maxPointsPart, + std::vector& 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(y); + const short *dIdy_row = dIdy.ptr(y); + uchar *texturedMask_row = texturedMask.ptr(y); + for(int x = 0; x < dIdx.cols; x++) + { + float magnitude2 = static_cast(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& pyramidDepth, std::vector& 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(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& pyramidNormals, const std::vector& pyramidMask, double maxPointsPart, + std::vector& 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(y); + uchar *normalsMask_row = pyramidNormalsMask[i].ptr(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(); + Eigen::Matrix 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(0,3) = ksi.at(3); + Rt.at(1,3) = ksi.at(4); + Rt.at(2,3) = ksi.at(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(); + + AutoBuffer 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(); + 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(v1); + const uchar *mask1_row = selectMask1.ptr(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(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(v0,u0); + if(validMask0.at(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff) + { + CV_DbgAssert(!cvIsNaN(d0)); + Vec2s& c = corresps.at(v0,u0); + if(c[0] != -1) + { + int exist_u1 = c[0], exist_v1 = c[1]; + + float exist_d1 = (float)(depth1.at(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(); + for(int v0 = 0, i = 0; v0 < corresps.rows; v0++) + { + const Vec2s* corresps_row = corresps.ptr(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(); + + const int correspsCount = corresps.rows; + + CV_Assert(Rt.type() == CV_64FC1); + const double * Rt_ptr = Rt.ptr(); + + AutoBuffer diffs(correspsCount); + float* diffs_ptr = diffs; + + const Vec4i* corresps_ptr = corresps.ptr(); + + 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(static_cast(image0.at(v0,u0)) - + static_cast(image1.at(v1,u1))); + sigma += diffs_ptr[correspIndex] * diffs_ptr[correspIndex]; + } + sigma = std::sqrt(sigma/correspsCount); + + std::vector 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(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(v1,u1), + w_sobelScale * dI_dy1.at(v1,u1), + tp0, fx, fy); + + for(int y = 0; y < transformDim; y++) + { + double* AtA_ptr = AtA.ptr(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(x,y) = AtA.at(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(); + + const int correspsCount = corresps.rows; + + CV_Assert(Rt.type() == CV_64FC1); + const double * Rt_ptr = Rt.ptr(); + + AutoBuffer diffs(correspsCount); + float * diffs_ptr = diffs; + + AutoBuffer transformedPoints0(correspsCount); + Point3f * tps0_ptr = transformedPoints0; + + const Vec4i* corresps_ptr = corresps.ptr(); + + 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(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(v1, u1); + Point3f v = cloud1.at(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 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(v1, u1) * w); + + for(int y = 0; y < transformDim; y++) + { + double* AtA_ptr = AtA.ptr(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(x,y) = AtA.at(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& srcFrame, + const Ptr& dstFrame, + const cv::Mat& cameraMatrix, + float maxDepthDiff, const std::vector& 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 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(0,0); + const double fy = levelCameraMatrix.at(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 +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 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::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(y); + const Point3f* transformedCloud_row = transformedCloud.ptr(y); + const Point2f* points2d_row = &points2d[y*image.cols]; + const ImageElemType* image_row = image.ptr(y); + const uchar* mask_row = mask.empty() ? 0 : mask.ptr(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(p2d) > transformed_z) + { + warpedImage.at(p2d) = image_row[x]; + zBuffer.at(p2d) = transformed_z; + } + } + } + + if(warpedMask) + *warpedMask = zBuffer != std::numeric_limits::max(); + + if(warpedDepth) + { + zBuffer.setTo(std::numeric_limits::quiet_NaN(), zBuffer == std::numeric_limits::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 srcFrame(new OdometryFrame(srcImage, srcDepth, srcMask)); + Ptr dstFrame(new OdometryFrame(dstImage, dstDepth, dstMask)); + + return compute(srcFrame, dstFrame, Rt, initRt); +} + +bool Odometry::compute(Ptr& srcFrame, Ptr& 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 &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& _iterCounts, + const std::vector& _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& 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 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& srcFrame, const Ptr& 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& _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& 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 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("rows") != frame->depth.rows || + normalsComputer->get("cols") != frame->depth.cols || + cv::norm(normalsComputer->get("K"), cameraMatrix) > FLT_EPSILON) + normalsComputer = cv::Ptr(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& srcFrame, const Ptr& 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& _iterCounts, + const std::vector& _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& 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 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("rows") != frame->depth.rows || + normalsComputer->get("cols") != frame->depth.cols || + cv::norm(normalsComputer->get("K"), cameraMatrix) > FLT_EPSILON) + normalsComputer = cv::Ptr(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& srcFrame, const Ptr& 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(image, depth, mask, Rt, cameraMatrix, distCoeff, warpedImage, warpedDepth, warpedMask); + else if(image.type() == CV_8UC3) + warpFrameImpl >(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 diff --git a/modules/rgbd/src/plane.cpp b/modules/rgbd/src/plane.cpp new file mode 100644 index 000000000..3a56c4d9c --- /dev/null +++ b/modules/rgbd/src/plane.cpp @@ -0,0 +1,641 @@ +/* + * 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 +#include + +#include + +/** 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(2, 0), svd.vt.at(2, 1), svd.vt.at(2, 2)); + mse_ = svd.w.at(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_ & 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(Q_.ptr < cv::Vec > (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(pointpointt); + m += (*vec); + ++K; + } + } + if (K == 0) + { + mse_(y, x) = std::numeric_limits::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(2, 0), svd.vt.at(2, 1), svd.vt.at(2, 2)); + mse_(y, x) = svd.w.at(2) / K; + } + } + + /** The size of the block */ + int block_size_; + cv::Mat_ m_; + cv::Mat_ n_; + cv::Mat_ > Q_; + cv::Mat_ 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_::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::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 tiles_; + /** contains 1 when the tiles has been studied, 0 otherwise */ + cv::Mat_ done_tiles_; +}; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +class InlierFinder +{ +public: + InlierFinder(float err, const cv::Mat_ & points3d, const cv::Mat_ & 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 & plane, TileQueue & tile_queue, + std::set & neighboring_tiles, cv::Mat_ & overall_mask, + cv::Mat_ & 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(plane_grid.Q_.ptr < cv::Vec + > (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 > pairs; + if (tile.x_ > 0) + for (unsigned char * val = overall_mask.ptr(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(tile.x_ - 1, tile.y_)); + break; + } + if (tile.x_ < plane_mask.cols - 1) + for (unsigned char * val = overall_mask.ptr(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(tile.x_ + 1, tile.y_)); + break; + } + if (tile.y_ > 0) + for (unsigned char * val = overall_mask.ptr(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(tile.x_, tile.y_ - 1)); + break; + } + if (tile.y_ < plane_mask.rows - 1) + for (unsigned char * val = overall_mask.ptr(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(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_ & points3d_; + const cv::Mat_ & 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_ 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_ mask_out_uc = (cv::Mat_&) 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 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 plane; + if ((sensor_error_a_ == 0) && (sensor_error_b_ == 0) && (sensor_error_c_ == 0)) + plane = cv::Ptr(new Plane(plane_grid.m_(y, x), n, index_plane)); + else + plane = cv::Ptr(new PlaneABC(plane_grid.m_(y, x), n, index_plane, sensor_error_a_, sensor_error_b_, sensor_error_c_)); + + cv::Mat_ plane_mask = cv::Mat_::zeros(points3d.rows / block_size_, + points3d.cols / block_size_); + std::set 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(0); + for(size_t i=0; i +#include +#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; + } +} diff --git a/modules/rgbd/src/utils.cpp b/modules/rgbd/src/utils.cpp new file mode 100644 index 000000000..e2be065cd --- /dev/null +++ b/modules/rgbd/src/utils.cpp @@ -0,0 +1,77 @@ +/* + * 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 +#include + +#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::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::min(); // Should we do std::numeric_limits::max() too ? + out.setTo(std::numeric_limits::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::min()) | (in == std::numeric_limits::max()); // Should we do std::numeric_limits::max() too ? + out.setTo(std::numeric_limits::quiet_NaN(), valid_mask); //set a$ + } + if ((in_depth == CV_32F) || (in_depth == CV_64F)) + in.convertTo(out, depth); + } +} diff --git a/modules/rgbd/src/utils.h b/modules/rgbd/src/utils.h new file mode 100644 index 000000000..8d4bc2e89 --- /dev/null +++ b/modules/rgbd/src/utils.h @@ -0,0 +1,73 @@ +/* + * 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 + +/** 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::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 +void +rescaleDepthTemplated(const cv::Mat& in, cv::Mat& out); + +template<> +inline void +rescaleDepthTemplated(const cv::Mat& in, cv::Mat& out) +{ + rescaleDepth(in, CV_32F, out); +} + +template<> +inline void +rescaleDepthTemplated(const cv::Mat& in, cv::Mat& out) +{ + rescaleDepth(in, CV_64F, out); +} + +#endif /* __cplusplus */ + +#endif + +/* End of file. */ diff --git a/modules/rgbd/test/test_main.cpp b/modules/rgbd/test/test_main.cpp new file mode 100644 index 000000000..d66d0ffc9 --- /dev/null +++ b/modules/rgbd/test/test_main.cpp @@ -0,0 +1,3 @@ +#include "test_precomp.hpp" + +CV_TEST_MAIN("rgbd") diff --git a/modules/rgbd/test/test_normal.cpp b/modules/rgbd/test/test_normal.cpp new file mode 100644 index 000000000..2bf1f7f4f --- /dev/null +++ b/modules/rgbd/test/test_normal.cpp @@ -0,0 +1,415 @@ +/* + * 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 + +#include +#include + +#include "test_precomp.hpp" + +cv::Point3f +rayPlaneIntersection(cv::Point2f uv, const cv::Mat& centroid, const cv::Mat& normal, const cv::Mat_& 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_& 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_(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& planes_out, cv::Mat_ &plane_mask, cv::Mat& points3d, cv::Mat& normals, + int n_planes); +void +gen_points_3d(std::vector& planes_out, cv::Mat_ &plane_mask, cv::Mat& points3d, cv::Mat& normals, + int n_planes) +{ + std::vector 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_ 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 > 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_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 channels; + cv::split(points3d, channels); + normals_computer(channels[2], in_normals); + } + else + normals_computer(points3d, in_normals); + tm.stop(); + + cv::Mat_ 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 planes; + cv::Mat points3d, ground_normals; + cv::Mat_ 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 & 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 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_ 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(); +} diff --git a/modules/rgbd/test/test_odometry.cpp b/modules/rgbd/test/test_odometry.cpp new file mode 100644 index 000000000..8fd723a96 --- /dev/null +++ b/modules/rgbd/test/test_odometry.cpp @@ -0,0 +1,350 @@ +/* + * 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 +#include +#include + +#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(y,x); + if(r.contains(p)) + { + float curDepth = warpedDepth.at(p.y, p.x); + float newDepth = warpedCloud.at(y, x).z; + if(newDepth < curDepth && newDepth > 0) + { + warpedImage.at(p.y, p.x) = image.at(y,x); + warpedDepth.at(p.y, p.x) = newDepth; + } + } + } + } + warpedDepth.setTo(std::numeric_limits::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(y,x)) || depth.at(y,x) > 10 || depth.at(y,x) <= FLT_EPSILON) + mask.at(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(y,x) && dilatedMask.at(y,x)) + { + image.at(y,x) = static_cast(0.5f * (static_cast(minImage.at(y,x)) + + static_cast(maxImage.at(y,x)))); + depth.at(y,x) = 0.5f * (minDepth.at(y,x) + maxDepth.at(y,x)); + } +} + +class CV_OdometryTest : public cvtest::BaseTest +{ +public: + CV_OdometryTest(const Ptr& _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; + 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::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(0,0) = fx; + K.at(1,1) = fy; + K.at(0,2) = cx; + K.at(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(better_1time_count) < maxError1 * static_cast(iterCount)) + { + ts->printf(cvtest::TS::LOG, "\nIncorrect count of accurate poses [1st case]: %f / %f", static_cast(better_1time_count), maxError1 * static_cast(iterCount)); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } + + if(static_cast(better_5times_count) < maxError5 * static_cast(iterCount)) + { + ts->printf(cvtest::TS::LOG, "\nIncorrect count of accurate poses [2nd case]: %f / %f", static_cast(better_5times_count), maxError5 * static_cast(iterCount)); + ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); + } +} + +/****************************************************************************************\ +* Tests registrations * +\****************************************************************************************/ + +TEST(RGBD_Odometry_Rgbd, algorithmic) +{ + CV_OdometryTest test(Algorithm::create("RGBD.RgbdOdometry"), 0.99, 0.94); + test.safe_run(); +} + +TEST(RGBD_Odometry_ICP, algorithmic) +{ + CV_OdometryTest test(Algorithm::create("RGBD.ICPOdometry"), 0.99, 0.99); + test.safe_run(); +} + +TEST(RGBD_Odometry_RgbdICP, algorithmic) +{ + CV_OdometryTest test(Algorithm::create("RGBD.RgbdICPOdometry"), 0.99, 0.99); + test.safe_run(); +} diff --git a/modules/rgbd/test/test_precomp.cpp b/modules/rgbd/test/test_precomp.cpp new file mode 100644 index 000000000..5956e13e3 --- /dev/null +++ b/modules/rgbd/test/test_precomp.cpp @@ -0,0 +1 @@ +#include "test_precomp.hpp" diff --git a/modules/rgbd/test/test_precomp.hpp b/modules/rgbd/test/test_precomp.hpp new file mode 100644 index 000000000..ccf662065 --- /dev/null +++ b/modules/rgbd/test/test_precomp.hpp @@ -0,0 +1,8 @@ +#ifndef __OPENCV_TEST_PRECOMP_HPP__ +#define __OPENCV_TEST_PRECOMP_HPP__ + +#include +#include +#include + +#endif diff --git a/modules/rgbd/test/test_utils.cpp b/modules/rgbd/test/test_utils.cpp new file mode 100644 index 000000000..3bbe03643 --- /dev/null +++ b/modules/rgbd/test/test_utils.cpp @@ -0,0 +1,61 @@ +#include + +#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_(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.); + + // Create a random depth image + cv::RNG rng; + cv::Mat_ 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_ 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_(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(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(); +} diff --git a/modules/rgbd/testdata/rgbd/odometry/depth.png b/modules/rgbd/testdata/rgbd/odometry/depth.png new file mode 100644 index 000000000..dd4364259 Binary files /dev/null and b/modules/rgbd/testdata/rgbd/odometry/depth.png differ diff --git a/modules/rgbd/testdata/rgbd/odometry/rgb.png b/modules/rgbd/testdata/rgbd/odometry/rgb.png new file mode 100644 index 000000000..22fb853d1 Binary files /dev/null and b/modules/rgbd/testdata/rgbd/odometry/rgb.png differ