Vincent Rabaud
11 years ago
parent
7f5a7bf3ac
commit
49a357b948
22 changed files with 5682 additions and 0 deletions
@ -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) |
||||
@ -0,0 +1,12 @@ |
||||
************************** |
||||
rgbd. RGB-Depth Processing |
||||
************************** |
||||
|
||||
.. highlight:: cpp |
||||
|
||||
.. toctree:: |
||||
:maxdepth: 2 |
||||
|
||||
depth_image |
||||
geometry |
||||
odometry |
||||
@ -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 <limits> |
||||
|
||||
#include <opencv2/core.hpp> |
||||
|
||||
namespace cv |
||||
{ |
||||
/** Checks if the value is a valid depth. For CV_16U or CV_16S, the convention is to be invalid if it is
|
||||
* a limit. For a float/double, we just check if it is a NaN |
||||
* @param depth the depth to check for validity |
||||
*/ |
||||
CV_EXPORTS |
||||
inline bool |
||||
isValidDepth(const float & depth) |
||||
{ |
||||
return !cvIsNaN(depth); |
||||
} |
||||
CV_EXPORTS |
||||
inline bool |
||||
isValidDepth(const double & depth) |
||||
{ |
||||
return !cvIsNaN(depth); |
||||
} |
||||
CV_EXPORTS |
||||
inline bool |
||||
isValidDepth(const short int & depth) |
||||
{ |
||||
return (depth != std::numeric_limits<short int>::min()) && (depth != std::numeric_limits<short int>::max()); |
||||
} |
||||
CV_EXPORTS |
||||
inline bool |
||||
isValidDepth(const unsigned short int & depth) |
||||
{ |
||||
return (depth != std::numeric_limits<unsigned short int>::min()) |
||||
&& (depth != std::numeric_limits<unsigned short int>::max()); |
||||
} |
||||
CV_EXPORTS |
||||
inline bool |
||||
isValidDepth(const int & depth) |
||||
{ |
||||
return (depth != std::numeric_limits<int>::min()) && (depth != std::numeric_limits<int>::max()); |
||||
} |
||||
CV_EXPORTS |
||||
inline bool |
||||
isValidDepth(const unsigned int & depth) |
||||
{ |
||||
return (depth != std::numeric_limits<unsigned int>::min()) && (depth != std::numeric_limits<unsigned int>::max()); |
||||
} |
||||
|
||||
/** Object that can compute the normals in an image.
|
||||
* It is an object as it can cache data for speed efficiency |
||||
* The implemented methods are either: |
||||
* - FALS (the fastest) and SRI from |
||||
* ``Fast and Accurate Computation of Surface Normals from Range Images`` |
||||
* by H. Badino, D. Huber, Y. Park and T. Kanade |
||||
* - the normals with bilateral filtering on a depth image from |
||||
* ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects`` |
||||
* by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit |
||||
*/ |
||||
class CV_EXPORTS RgbdNormals: public Algorithm |
||||
{ |
||||
public: |
||||
enum RGBD_NORMALS_METHOD |
||||
{ |
||||
RGBD_NORMALS_METHOD_FALS, RGBD_NORMALS_METHOD_LINEMOD, RGBD_NORMALS_METHOD_SRI |
||||
}; |
||||
|
||||
RgbdNormals() |
||||
: |
||||
rows_(0), |
||||
cols_(0), |
||||
depth_(0), |
||||
K_(Mat()), |
||||
window_size_(0), |
||||
method_(RGBD_NORMALS_METHOD_FALS), |
||||
rgbd_normals_impl_(0) |
||||
{ |
||||
} |
||||
|
||||
/** Constructor
|
||||
* @param rows the number of rows of the depth image normals will be computed on |
||||
* @param cols the number of cols of the depth image normals will be computed on |
||||
* @param depth the depth of the normals (only CV_32F or CV_64F for FALS and SRI, CV_16U for LINEMOD) |
||||
* @param K the calibration matrix to use |
||||
* @param window_size the window size to compute the normals: can only be 1,3,5 or 7 |
||||
* @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS |
||||
*/ |
||||
RgbdNormals(int rows, int cols, int depth, InputArray K, int window_size = 5, int method = |
||||
RGBD_NORMALS_METHOD_FALS); |
||||
|
||||
~RgbdNormals(); |
||||
|
||||
AlgorithmInfo* |
||||
info() const; |
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point.
|
||||
* @param points a rows x cols x 3 matrix of CV_32F/CV64F or a rows x cols x 1 CV_U16S |
||||
* @param normals a rows x cols x 3 matrix |
||||
*/ |
||||
void |
||||
operator()(InputArray points, OutputArray normals) const; |
||||
|
||||
/** Initializes some data that is cached for later computation
|
||||
* If that function is not called, it will be called the first time normals are computed |
||||
*/ |
||||
void |
||||
initialize() const; |
||||
|
||||
/** Return the current method in that normal computer
|
||||
* @return |
||||
*/ |
||||
int |
||||
method() const |
||||
{ |
||||
return method_; |
||||
} |
||||
protected: |
||||
void |
||||
initialize_normals_impl(int rows, int cols, int depth, const Mat & K, int window_size, int method) const; |
||||
|
||||
int rows_, cols_, depth_; |
||||
Mat K_; |
||||
int window_size_; |
||||
int method_; |
||||
mutable void* rgbd_normals_impl_; |
||||
}; |
||||
|
||||
/** Object that can clean a noisy depth image
|
||||
*/ |
||||
class CV_EXPORTS DepthCleaner: public Algorithm |
||||
{ |
||||
public: |
||||
/** NIL method is from
|
||||
* ``Modeling Kinect Sensor Noise for Improved 3d Reconstruction and Tracking`` |
||||
* by C. Nguyen, S. Izadi, D. Lovel |
||||
*/ |
||||
enum DEPTH_CLEANER_METHOD |
||||
{ |
||||
DEPTH_CLEANER_NIL |
||||
}; |
||||
|
||||
DepthCleaner() |
||||
: |
||||
depth_(0), |
||||
window_size_(0), |
||||
method_(DEPTH_CLEANER_NIL), |
||||
depth_cleaner_impl_(0) |
||||
{ |
||||
} |
||||
|
||||
/** Constructor
|
||||
* @param depth the depth of the normals (only CV_32F or CV_64F for FALS and SRI, CV_16U for LINEMOD) |
||||
* @param window_size the window size to compute the normals: can only be 1,3,5 or 7 |
||||
* @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS |
||||
*/ |
||||
DepthCleaner(int depth, int window_size = 5, int method = DEPTH_CLEANER_NIL); |
||||
|
||||
~DepthCleaner(); |
||||
|
||||
AlgorithmInfo* |
||||
info() const; |
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point.
|
||||
* @param points a rows x cols x 3 matrix of CV_32F/CV64F or a rows x cols x 1 CV_U16S |
||||
* @param depth a rows x cols matrix of the cleaned up depth |
||||
*/ |
||||
void |
||||
operator()(InputArray points, OutputArray depth) const; |
||||
|
||||
/** Initializes some data that is cached for later computation
|
||||
* If that function is not called, it will be called the first time normals are computed |
||||
*/ |
||||
void |
||||
initialize() const; |
||||
|
||||
/** Return the current method in that normal computer
|
||||
* @return |
||||
*/ |
||||
int |
||||
method() const |
||||
{ |
||||
return method_; |
||||
} |
||||
protected: |
||||
void |
||||
initialize_cleaner_impl() const; |
||||
|
||||
int depth_; |
||||
int window_size_; |
||||
int method_; |
||||
mutable void* depth_cleaner_impl_; |
||||
}; |
||||
|
||||
/**
|
||||
* @param depth the depth image |
||||
* @param K |
||||
* @param in_points the list of xy coordinates |
||||
* @param points3d the resulting 3d points |
||||
*/ |
||||
CV_EXPORTS |
||||
void |
||||
depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d); |
||||
|
||||
/** Converts a depth image to an organized set of 3d points.
|
||||
* The coordinate system is x pointing left, y down and z away from the camera |
||||
* @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters |
||||
* (as done with the Microsoft Kinect), otherwise, if given as CV_32F or CV_64F, it is assumed in meters) |
||||
* @param K The calibration matrix |
||||
* @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the |
||||
* depth of `K` if `depth` is of depth CV_U |
||||
* @param mask the mask of the points to consider (can be empty) |
||||
*/ |
||||
CV_EXPORTS |
||||
void |
||||
depthTo3d(InputArray depth, InputArray K, OutputArray points3d, InputArray mask = noArray()); |
||||
|
||||
/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
|
||||
* by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN() |
||||
* Otherwise, the image is simply converted to floats |
||||
* @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters |
||||
* (as done with the Microsoft Kinect), it is assumed in meters) |
||||
* @param the desired output depth (floats or double) |
||||
* @param out The rescaled float depth image |
||||
*/ |
||||
CV_EXPORTS |
||||
void |
||||
rescaleDepth(InputArray in, int depth, OutputArray out); |
||||
|
||||
/** Object that can compute planes in an image
|
||||
*/ |
||||
class CV_EXPORTS RgbdPlane: public Algorithm |
||||
{ |
||||
public: |
||||
enum RGBD_PLANE_METHOD |
||||
{ |
||||
RGBD_PLANE_METHOD_DEFAULT |
||||
}; |
||||
|
||||
RgbdPlane(RGBD_PLANE_METHOD method = RGBD_PLANE_METHOD_DEFAULT) |
||||
: |
||||
method_(method), |
||||
block_size_(40), |
||||
min_size_(block_size_*block_size_), |
||||
threshold_(0.01), |
||||
sensor_error_a_(0), |
||||
sensor_error_b_(0), |
||||
sensor_error_c_(0) |
||||
{ |
||||
} |
||||
|
||||
/** Find The planes in a depth image
|
||||
* @param points3d the 3d points organized like the depth image: rows x cols with 3 channels |
||||
* @param the normals for every point in the depth image |
||||
* @param mask An image where each pixel is labeled with the plane it belongs to |
||||
* and 255 if it does not belong to any plane |
||||
* @param the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0, norm(a,b,c)=1 |
||||
* and c < 0 (so that the normal points towards the camera) |
||||
*/ |
||||
void |
||||
operator()(InputArray points3d, InputArray normals, OutputArray mask, |
||||
OutputArray plane_coefficients); |
||||
|
||||
/** Find The planes in a depth image but without doing a normal check, which is faster but less accurate
|
||||
* @param points3d the 3d points organized like the depth image: rows x cols with 3 channels |
||||
* @param mask An image where each pixel is labeled with the plane it belongs to |
||||
* and 255 if it does not belong to any plane |
||||
* @param the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0 |
||||
*/ |
||||
void |
||||
operator()(InputArray points3d, OutputArray mask, OutputArray plane_coefficients); |
||||
|
||||
AlgorithmInfo* |
||||
info() const; |
||||
private: |
||||
/** The method to use to compute the planes */ |
||||
int method_; |
||||
/** The size of the blocks to look at for a stable MSE */ |
||||
int block_size_; |
||||
/** The minimum size of a cluster to be considered a plane */ |
||||
int min_size_; |
||||
/** How far a point can be from a plane to belong to it (in meters) */ |
||||
double threshold_; |
||||
/** coefficient of the sensor error with respect to the. All 0 by default but you want a=0.0075 for a Kinect */ |
||||
double sensor_error_a_, sensor_error_b_, sensor_error_c_; |
||||
}; |
||||
|
||||
/** Object that contains a frame data.
|
||||
*/ |
||||
struct CV_EXPORTS RgbdFrame |
||||
{ |
||||
RgbdFrame(); |
||||
RgbdFrame(const Mat& image, const Mat& depth, const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1); |
||||
virtual ~RgbdFrame(); |
||||
|
||||
virtual void |
||||
release(); |
||||
|
||||
int ID; |
||||
Mat image; |
||||
Mat depth; |
||||
Mat mask; |
||||
Mat normals; |
||||
}; |
||||
|
||||
/** Object that contains a frame data that is possibly needed for the Odometry.
|
||||
* It's used for the efficiency (to pass precomputed/cached data of the frame that participates |
||||
* in the Odometry processing several times). |
||||
*/ |
||||
struct CV_EXPORTS OdometryFrame : public RgbdFrame |
||||
{ |
||||
/** These constants are used to set a type of cache which has to be prepared depending on the frame role:
|
||||
* srcFrame or dstFrame (see compute method of the Odometry class). For the srcFrame and dstFrame different cache data may be required, |
||||
* some part of a cache may be common for both frame roles. |
||||
* @param CACHE_SRC The cache data for the srcFrame will be prepared. |
||||
* @param CACHE_DST The cache data for the dstFrame will be prepared. |
||||
* @param CACHE_ALL The cache data for both srcFrame and dstFrame roles will be computed. |
||||
*/ |
||||
enum
|
||||
{ |
||||
CACHE_SRC = 1, CACHE_DST = 2, CACHE_ALL = CACHE_SRC + CACHE_DST |
||||
}; |
||||
|
||||
OdometryFrame(); |
||||
OdometryFrame(const Mat& image, const Mat& depth, const Mat& mask=Mat(), const Mat& normals=Mat(), int ID=-1); |
||||
|
||||
virtual void |
||||
release(); |
||||
|
||||
void |
||||
releasePyramids(); |
||||
|
||||
std::vector<Mat> pyramidImage; |
||||
std::vector<Mat> pyramidDepth; |
||||
std::vector<Mat> pyramidMask; |
||||
|
||||
std::vector<Mat> pyramidCloud; |
||||
|
||||
std::vector<Mat> pyramid_dI_dx; |
||||
std::vector<Mat> pyramid_dI_dy; |
||||
std::vector<Mat> pyramidTexturedMask; |
||||
|
||||
std::vector<Mat> pyramidNormals; |
||||
std::vector<Mat> pyramidNormalsMask; |
||||
}; |
||||
|
||||
/** Base class for computation of odometry.
|
||||
*/ |
||||
class CV_EXPORTS Odometry: public Algorithm |
||||
{ |
||||
public: |
||||
|
||||
/** A class of transformation*/ |
||||
enum
|
||||
{ |
||||
ROTATION = 1, TRANSLATION = 2, RIGID_BODY_MOTION = 4 |
||||
}; |
||||
|
||||
static inline float |
||||
DEFAULT_MIN_DEPTH() |
||||
{ |
||||
return 0.f; // in meters
|
||||
} |
||||
static inline float |
||||
DEFAULT_MAX_DEPTH() |
||||
{ |
||||
return 4.f; // in meters
|
||||
} |
||||
static inline float |
||||
DEFAULT_MAX_DEPTH_DIFF() |
||||
{ |
||||
return 0.07f; // in meters
|
||||
} |
||||
static inline float |
||||
DEFAULT_MAX_POINTS_PART() |
||||
{ |
||||
return 0.07f; // in [0, 1]
|
||||
} |
||||
static inline float |
||||
DEFAULT_MAX_TRANSLATION() |
||||
{ |
||||
return 0.15f; // in meters
|
||||
} |
||||
static inline float |
||||
DEFAULT_MAX_ROTATION() |
||||
{ |
||||
return 15; // in degrees
|
||||
} |
||||
|
||||
/** Method to compute a transformation from the source frame to the destination one.
|
||||
* Some odometry algorithms do not used some data of frames (eg. ICP does not use images). |
||||
* In such case corresponding arguments can be set as empty cv::Mat. |
||||
* The method returns true if all internal computions were possible (e.g. there were enough correspondences,
|
||||
* system of equations has a solution, etc) and resulting transformation satisfies some test if it's provided |
||||
* by the Odometry inheritor implementation (e.g. thresholds for maximum translation and rotation). |
||||
* @param srcImage Image data of the source frame (CV_8UC1) |
||||
* @param srcDepth Depth data of the source frame (CV_32FC1, in meters) |
||||
* @param srcMask Mask that sets which pixels have to be used from the source frame (CV_8UC1) |
||||
* @param dstImage Image data of the destination frame (CV_8UC1) |
||||
* @param dstDepth Depth data of the destination frame (CV_32FC1, in meters) |
||||
* @param dstMask Mask that sets which pixels have to be used from the destination frame (CV_8UC1) |
||||
* @param Rt Resulting transformation from the source frame to the destination one (rigid body motion): |
||||
dst_p = Rt * src_p, where dst_p is a homogeneous point in the destination frame and src_p is |
||||
homogeneous point in the source frame, |
||||
Rt is 4x4 matrix of CV_64FC1 type. |
||||
* @param initRt Initial transformation from the source frame to the destination one (optional) |
||||
*/ |
||||
bool |
||||
compute(const Mat& srcImage, const Mat& srcDepth, const Mat& srcMask, const Mat& dstImage, const Mat& dstDepth, |
||||
const Mat& dstMask, Mat& Rt, const Mat& initRt = Mat()) const; |
||||
|
||||
/** One more method to compute a transformation from the source frame to the destination one.
|
||||
* It is designed to save on computing the frame data (image pyramids, normals, etc.). |
||||
*/ |
||||
bool |
||||
compute(Ptr<OdometryFrame>& srcFrame, Ptr<OdometryFrame>& dstFrame, Mat& Rt, const Mat& initRt = Mat()) const; |
||||
|
||||
/** Prepare a cache for the frame. The function checks the precomputed/passed data (throws the error if this data
|
||||
* does not satisfy) and computes all remaining cache data needed for the frame. Returned size is a resolution |
||||
* of the prepared frame. |
||||
* @param odometry The odometry which will process the frame. |
||||
* @param cacheType The cache type: CACHE_SRC, CACHE_DST or CACHE_ALL. |
||||
*/ |
||||
virtual Size |
||||
prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const; |
||||
|
||||
protected: |
||||
virtual void |
||||
checkParams() const = 0; |
||||
|
||||
virtual bool |
||||
computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, |
||||
const Mat& initRt) const = 0; |
||||
}; |
||||
|
||||
/** Odometry based on the paper "Real-Time Visual Odometry from Dense RGB-D Images",
|
||||
* F. Steinbucker, J. Strum, D. Cremers, ICCV, 2011. |
||||
*/ |
||||
class CV_EXPORTS RgbdOdometry: public Odometry |
||||
{ |
||||
public: |
||||
RgbdOdometry(); |
||||
/** Constructor.
|
||||
* @param cameraMatrix Camera matrix |
||||
* @param minDepth Pixels with depth less than minDepth will not be used (in meters) |
||||
* @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters) |
||||
* @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out |
||||
* if their depth difference is larger than maxDepthDiff (in meters) |
||||
* @param iterCounts Count of iterations on each pyramid level. |
||||
* @param minGradientMagnitudes For each pyramid level the pixels will be filtered out |
||||
* if they have gradient magnitude less than minGradientMagnitudes[level]. |
||||
*/ |
||||
RgbdOdometry(const Mat& cameraMatrix, float minDepth = DEFAULT_MIN_DEPTH(), float maxDepth = DEFAULT_MAX_DEPTH(), |
||||
float maxDepthDiff = DEFAULT_MAX_DEPTH_DIFF(), const std::vector<int>& iterCounts = std::vector<int>(), |
||||
const std::vector<float>& minGradientMagnitudes = std::vector<float>(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(), |
||||
int transformType = RIGID_BODY_MOTION); |
||||
|
||||
virtual Size |
||||
prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const; |
||||
|
||||
AlgorithmInfo* |
||||
info() const; |
||||
|
||||
protected: |
||||
virtual void |
||||
checkParams() const; |
||||
|
||||
virtual bool |
||||
computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, |
||||
const Mat& initRt) const; |
||||
|
||||
// Some params have commented desired type. It's due to cv::AlgorithmInfo::addParams does not support it now.
|
||||
/*float*/ |
||||
double minDepth, maxDepth, maxDepthDiff; |
||||
/*vector<int>*/ |
||||
Mat iterCounts; |
||||
/*vector<float>*/ |
||||
Mat minGradientMagnitudes; |
||||
double maxPointsPart; |
||||
|
||||
Mat cameraMatrix; |
||||
int transformType; |
||||
|
||||
double maxTranslation, maxRotation; |
||||
}; |
||||
|
||||
/** Odometry based on the paper "KinectFusion: Real-Time Dense Surface Mapping and Tracking",
|
||||
* Richard A. Newcombe, Andrew Fitzgibbon, at al, SIGGRAPH, 2011. |
||||
*/ |
||||
class ICPOdometry: public Odometry |
||||
{ |
||||
public: |
||||
ICPOdometry(); |
||||
/** Constructor.
|
||||
* @param cameraMatrix Camera matrix |
||||
* @param minDepth Pixels with depth less than minDepth will not be used |
||||
* @param maxDepth Pixels with depth larger than maxDepth will not be used |
||||
* @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out |
||||
* if their depth difference is larger than maxDepthDiff |
||||
* @param pointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart |
||||
* @param iterCounts Count of iterations on each pyramid level. |
||||
*/ |
||||
ICPOdometry(const Mat& cameraMatrix, float minDepth = DEFAULT_MIN_DEPTH(), float maxDepth = DEFAULT_MAX_DEPTH(), |
||||
float maxDepthDiff = DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(), |
||||
const std::vector<int>& iterCounts = std::vector<int>(), int transformType = RIGID_BODY_MOTION); |
||||
|
||||
virtual Size |
||||
prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const; |
||||
|
||||
AlgorithmInfo* |
||||
info() const; |
||||
|
||||
protected: |
||||
virtual void |
||||
checkParams() const; |
||||
|
||||
virtual bool |
||||
computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, |
||||
const Mat& initRt) const; |
||||
|
||||
// Some params have commented desired type. It's due to cv::AlgorithmInfo::addParams does not support it now.
|
||||
/*float*/ |
||||
double minDepth, maxDepth, maxDepthDiff; |
||||
/*float*/ |
||||
double maxPointsPart; |
||||
/*vector<int>*/ |
||||
Mat iterCounts; |
||||
|
||||
Mat cameraMatrix; |
||||
int transformType; |
||||
|
||||
double maxTranslation, maxRotation; |
||||
|
||||
mutable cv::Ptr<cv::RgbdNormals> normalsComputer; |
||||
}; |
||||
|
||||
/** Odometry that merges RgbdOdometry and ICPOdometry by minimize sum of their energy functions.
|
||||
*/ |
||||
|
||||
class RgbdICPOdometry: public Odometry |
||||
{ |
||||
public: |
||||
RgbdICPOdometry(); |
||||
/** Constructor.
|
||||
* @param cameraMatrix Camera matrix |
||||
* @param minDepth Pixels with depth less than minDepth will not be used |
||||
* @param maxDepth Pixels with depth larger than maxDepth will not be used |
||||
* @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out |
||||
* if their depth difference is larger than maxDepthDiff |
||||
* @param pointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart |
||||
* @param iterCounts Count of iterations on each pyramid level. |
||||
* @param minGradientMagnitudes For each pyramid level the pixels will be filtered out |
||||
* if they have gradient magnitude less than minGradientMagnitudes[level]. |
||||
*/ |
||||
RgbdICPOdometry(const Mat& cameraMatrix, float minDepth = DEFAULT_MIN_DEPTH(), float maxDepth = DEFAULT_MAX_DEPTH(), |
||||
float maxDepthDiff = DEFAULT_MAX_DEPTH_DIFF(), float maxPointsPart = DEFAULT_MAX_POINTS_PART(), |
||||
const std::vector<int>& iterCounts = std::vector<int>(), |
||||
const std::vector<float>& minGradientMagnitudes = std::vector<float>(), |
||||
int transformType = RIGID_BODY_MOTION); |
||||
|
||||
virtual Size |
||||
prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const; |
||||
|
||||
AlgorithmInfo* |
||||
info() const; |
||||
|
||||
protected: |
||||
virtual void |
||||
checkParams() const; |
||||
|
||||
virtual bool |
||||
computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame, Mat& Rt, |
||||
const Mat& initRt) const; |
||||
|
||||
// Some params have commented desired type. It's due to cv::AlgorithmInfo::addParams does not support it now.
|
||||
/*float*/ |
||||
double minDepth, maxDepth, maxDepthDiff; |
||||
/*float*/ |
||||
double maxPointsPart; |
||||
/*vector<int>*/ |
||||
Mat iterCounts; |
||||
/*vector<float>*/ |
||||
Mat minGradientMagnitudes; |
||||
|
||||
Mat cameraMatrix; |
||||
int transformType; |
||||
|
||||
double maxTranslation, maxRotation; |
||||
|
||||
mutable cv::Ptr<cv::RgbdNormals> normalsComputer; |
||||
}; |
||||
|
||||
/** Warp the image: compute 3d points from the depth, transform them using given transformation,
|
||||
* then project color point cloud to an image plane.
|
||||
* This function can be used to visualize results of the Odometry algorithm. |
||||
* @param image The image (of CV_8UC1 or CV_8UC3 type) |
||||
* @param depth The depth (of type used in depthTo3d fuction) |
||||
* @param mask The mask of used pixels (of CV_8UC1), it can be empty |
||||
* @param Rt The transformation that will be applied to the 3d points computed from the depth |
||||
* @param cameraMatrix Camera matrix |
||||
* @param distCoeff Distortion coefficients |
||||
* @param warpedImage The warped image. |
||||
* @param warpedDepth The warped depth. |
||||
* @param warpedMask The warped mask. |
||||
*/ |
||||
CV_EXPORTS |
||||
void |
||||
warpFrame(const Mat& image, const Mat& depth, const Mat& mask, const Mat& Rt, const Mat& cameraMatrix, |
||||
const Mat& distCoeff, Mat& warpedImage, Mat* warpedDepth = 0, Mat* warpedMask = 0); |
||||
|
||||
// TODO Depth interpolation
|
||||
// Curvature
|
||||
// Get rescaleDepth return dubles if asked for
|
||||
} /* namespace cv */ |
||||
|
||||
#endif /* __cplusplus */ |
||||
|
||||
#endif |
||||
|
||||
/* End of file. */ |
||||
@ -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}) |
||||
@ -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 <opencv2/rgbd/rgbd.hpp> |
||||
|
||||
#include "opencv2/highgui/highgui.hpp" |
||||
#include "opencv2/contrib/contrib.hpp" |
||||
#include "opencv2/calib3d/calib3d.hpp" |
||||
|
||||
#include <dirent.h> |
||||
#include <iostream> |
||||
#include <fstream> |
||||
|
||||
using namespace std; |
||||
using namespace cv; |
||||
|
||||
#define BILATERAL_FILTER 0// if 1 then bilateral filter will be used for the depth
|
||||
|
||||
static |
||||
void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt ) |
||||
{ |
||||
CV_Assert( timestamps.size() == Rt.size() ); |
||||
|
||||
ofstream file( filename.c_str() ); |
||||
if( !file.is_open() ) |
||||
return; |
||||
|
||||
cout.precision(4); |
||||
for( size_t i = 0; i < Rt.size(); i++ ) |
||||
{ |
||||
const Mat& Rt_curr = Rt[i]; |
||||
if( Rt_curr.empty() ) |
||||
continue; |
||||
|
||||
CV_Assert( Rt_curr.type() == CV_64FC1 ); |
||||
|
||||
Mat R = Rt_curr(Rect(0,0,3,3)), rvec; |
||||
Rodrigues(R, rvec); |
||||
double alpha = norm( rvec ); |
||||
if(alpha > DBL_MIN) |
||||
rvec = rvec / alpha; |
||||
|
||||
double cos_alpha2 = std::cos(0.5 * alpha); |
||||
double sin_alpha2 = std::sin(0.5 * alpha); |
||||
|
||||
rvec *= sin_alpha2; |
||||
|
||||
CV_Assert( rvec.type() == CV_64FC1 ); |
||||
// timestamp tx ty tz qx qy qz qw
|
||||
file << timestamps[i] << " " << fixed |
||||
<< Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " " |
||||
<< rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl; |
||||
|
||||
} |
||||
file.close(); |
||||
} |
||||
|
||||
static |
||||
void setCameraMatrixFreiburg1(float& fx, float& fy, float& cx, float& cy) |
||||
{ |
||||
fx = 517.3f; fy = 516.5f; cx = 318.6f; cy = 255.3f; |
||||
} |
||||
|
||||
static |
||||
void setCameraMatrixFreiburg2(float& fx, float& fy, float& cx, float& cy) |
||||
{ |
||||
fx = 520.9f; fy = 521.0f; cx = 325.1f; cy = 249.7f; |
||||
} |
||||
|
||||
/*
|
||||
* This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
|
||||
* At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory |
||||
* to file of format that the benchmark requires. Saved file can be used for online evaluation. |
||||
*/ |
||||
int main(int argc, char** argv) |
||||
{ |
||||
if(argc != 4) |
||||
{ |
||||
cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP]" << endl; |
||||
return -1; |
||||
} |
||||
|
||||
vector<string> timestamps; |
||||
vector<Mat> Rts; |
||||
|
||||
const string filename = argv[1]; |
||||
ifstream file( filename.c_str() ); |
||||
if( !file.is_open() ) |
||||
return -1; |
||||
|
||||
char dlmrt = '/'; |
||||
size_t pos = filename.rfind(dlmrt); |
||||
string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt; |
||||
|
||||
const int timestampLength = 17; |
||||
const int rgbPathLehgth = 17+8; |
||||
const int depthPathLehgth = 17+10; |
||||
|
||||
float fx = 525.0f, // default
|
||||
fy = 525.0f, |
||||
cx = 319.5f, |
||||
cy = 239.5f; |
||||
if(filename.find("freiburg1") != string::npos) |
||||
setCameraMatrixFreiburg1(fx, fy, cx, cy); |
||||
if(filename.find("freiburg2") != string::npos) |
||||
setCameraMatrixFreiburg2(fx, fy, cx, cy); |
||||
Mat cameraMatrix = Mat::eye(3,3,CV_32FC1); |
||||
{ |
||||
cameraMatrix.at<float>(0,0) = fx; |
||||
cameraMatrix.at<float>(1,1) = fy; |
||||
cameraMatrix.at<float>(0,2) = cx; |
||||
cameraMatrix.at<float>(1,2) = cy; |
||||
} |
||||
|
||||
Ptr<OdometryFrame> frame_prev = new OdometryFrame(), |
||||
frame_curr = new OdometryFrame(); |
||||
Ptr<Odometry> odometry = Algorithm::create<Odometry>("RGBD." + string(argv[3]) + "Odometry"); |
||||
if(odometry.empty()) |
||||
{ |
||||
cout << "Can not create Odometry algorithm. Check the passed odometry name." << endl; |
||||
return -1; |
||||
} |
||||
odometry->set("cameraMatrix", cameraMatrix); |
||||
|
||||
TickMeter gtm; |
||||
int count = 0; |
||||
for(int i = 0; !file.eof(); i++) |
||||
{ |
||||
string str; |
||||
std::getline(file, str); |
||||
if(str.empty()) break; |
||||
if(str.at(0) == '#') continue; /* comment */ |
||||
|
||||
Mat image, depth; |
||||
// Read one pair (rgb and depth)
|
||||
// example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
|
||||
#if BILATERAL_FILTER |
||||
TickMeter tm_bilateral_filter; |
||||
#endif |
||||
{ |
||||
string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth ); |
||||
string timestap = str.substr(0, timestampLength); |
||||
string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth ); |
||||
|
||||
image = imread(dirname + rgbFilename); |
||||
depth = imread(dirname + depthFilename, -1); |
||||
|
||||
CV_Assert(!image.empty()); |
||||
CV_Assert(!depth.empty()); |
||||
CV_Assert(depth.type() == CV_16UC1); |
||||
|
||||
cout << i << " " << rgbFilename << " " << depthFilename << endl; |
||||
|
||||
// scale depth
|
||||
Mat depth_flt; |
||||
depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f); |
||||
#if not BILATERAL_FILTER |
||||
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0); |
||||
depth = depth_flt; |
||||
#else |
||||
tm_bilateral_filter.start(); |
||||
depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0)); |
||||
const double depth_sigma = 0.03; |
||||
const double space_sigma = 4.5; // in pixels
|
||||
Mat invalidDepthMask = depth_flt == 0.f; |
||||
depth_flt.setTo(-5*depth_sigma, invalidDepthMask); |
||||
bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma); |
||||
depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask); |
||||
tm_bilateral_filter.stop(); |
||||
cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl; |
||||
#endif |
||||
timestamps.push_back( timestap ); |
||||
} |
||||
|
||||
{ |
||||
Mat gray; |
||||
cvtColor(image, gray, CV_BGR2GRAY); |
||||
frame_curr->image = gray; |
||||
frame_curr->depth = depth; |
||||
|
||||
Mat Rt; |
||||
if(!Rts.empty()) |
||||
{ |
||||
TickMeter tm; |
||||
tm.start(); |
||||
gtm.start(); |
||||
bool res = odometry->compute(frame_curr, frame_prev, Rt); |
||||
gtm.stop(); |
||||
tm.stop(); |
||||
count++; |
||||
cout << "Time " << tm.getTimeSec() << endl; |
||||
#if BILATERAL_FILTER |
||||
cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl; |
||||
#endif |
||||
if(!res) |
||||
Rt = Mat::eye(4,4,CV_64FC1); |
||||
} |
||||
|
||||
if( Rts.empty() ) |
||||
Rts.push_back(Mat::eye(4,4,CV_64FC1)); |
||||
else |
||||
{ |
||||
Mat& prevRt = *Rts.rbegin(); |
||||
cout << "Rt " << Rt << endl; |
||||
Rts.push_back( prevRt * Rt ); |
||||
} |
||||
|
||||
if(!frame_prev.empty()) |
||||
frame_prev->release(); |
||||
std::swap(frame_prev, frame_curr); |
||||
} |
||||
} |
||||
|
||||
std::cout << "Average time " << gtm.getTimeSec()/count << std::endl; |
||||
writeResults(argv[2], timestamps, Rts); |
||||
|
||||
return 0; |
||||
} |
||||
@ -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 <opencv2/calib3d.hpp> |
||||
#include <opencv2/imgproc.hpp> |
||||
#include <opencv2/rgbd.hpp> |
||||
#include <iostream> |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace |
||||
{ |
||||
class DepthCleanerImpl |
||||
{ |
||||
public: |
||||
DepthCleanerImpl(int window_size, int depth, cv::DepthCleaner::DEPTH_CLEANER_METHOD method) |
||||
: |
||||
depth_(depth), |
||||
window_size_(window_size), |
||||
method_(method) |
||||
{ |
||||
} |
||||
|
||||
virtual |
||||
~DepthCleanerImpl() |
||||
{ |
||||
} |
||||
|
||||
virtual void |
||||
cache()=0; |
||||
|
||||
bool |
||||
validate(int depth, int window_size, int method) const |
||||
{ |
||||
return (window_size == window_size_) && (depth == depth_) && (method == method_); |
||||
} |
||||
protected: |
||||
int depth_; |
||||
int window_size_; |
||||
cv::DepthCleaner::DEPTH_CLEANER_METHOD method_; |
||||
}; |
||||
} |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace |
||||
{ |
||||
/** Given a depth image, compute the normals as detailed in the LINEMOD paper
|
||||
* ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects`` |
||||
* by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit |
||||
*/ |
||||
template<typename T> |
||||
class NIL: public DepthCleanerImpl |
||||
{ |
||||
public: |
||||
typedef cv::Vec<T, 3> Vec3T; |
||||
typedef cv::Matx<T, 3, 3> Mat33T; |
||||
|
||||
NIL(int window_size, int depth, cv::DepthCleaner::DEPTH_CLEANER_METHOD method) |
||||
: |
||||
DepthCleanerImpl(window_size, depth, method) |
||||
{ |
||||
} |
||||
|
||||
/** Compute cached data
|
||||
*/ |
||||
virtual void |
||||
cache() |
||||
{ |
||||
} |
||||
|
||||
/** Compute the normals
|
||||
* @param r |
||||
* @return |
||||
*/ |
||||
void |
||||
compute(const cv::Mat& depth_in, cv::Mat& depth_out) const |
||||
{ |
||||
switch (depth_in.depth()) |
||||
{ |
||||
case CV_16U: |
||||
{ |
||||
const cv::Mat_<unsigned short> &depth(depth_in); |
||||
cv::Mat depth_out_tmp; |
||||
computeImpl<unsigned short, float>(depth, depth_out_tmp, 0.001); |
||||
depth_out_tmp.convertTo(depth_out, CV_16U); |
||||
break; |
||||
} |
||||
case CV_32F: |
||||
{ |
||||
const cv::Mat_<float> &depth(depth_in); |
||||
computeImpl<float, float>(depth, depth_out, 1); |
||||
break; |
||||
} |
||||
case CV_64F: |
||||
{ |
||||
const cv::Mat_<double> &depth(depth_in); |
||||
computeImpl<double, double>(depth, depth_out, 1); |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
private: |
||||
/** Compute the normals
|
||||
* @param r |
||||
* @return |
||||
*/ |
||||
template<typename DepthDepth, typename ContainerDepth> |
||||
void |
||||
computeImpl(const cv::Mat_<DepthDepth> &depth_in, cv::Mat & depth_out, ContainerDepth scale) const |
||||
{ |
||||
const ContainerDepth theta_mean = 30. * CV_PI / 180; |
||||
int rows = depth_in.rows; |
||||
int cols = depth_in.cols; |
||||
|
||||
// Precompute some data
|
||||
const ContainerDepth sigma_L = 0.8 + 0.035 * theta_mean / (CV_PI / 2 - theta_mean); |
||||
cv::Mat_<ContainerDepth> sigma_z(rows, cols); |
||||
for (int y = 0; y < rows; ++y) |
||||
for (int x = 0; x < cols; ++x) |
||||
sigma_z(y, x) = 0.0012 + 0.0019 * (depth_in(y, x) * scale - 0.4) * (depth_in(y, x) * scale - 0.4); |
||||
|
||||
ContainerDepth difference_threshold = 10; |
||||
cv::Mat_<ContainerDepth> Dw_sum = cv::Mat_<ContainerDepth>::zeros(rows, cols), w_sum = |
||||
cv::Mat_<ContainerDepth>::zeros(rows, cols); |
||||
for (int y = 0; y < rows - 1; ++y) |
||||
{ |
||||
// Every pixel has had the contribution of previous pixels (in a row-major way)
|
||||
for (int x = 1; x < cols - 1; ++x) |
||||
{ |
||||
for (int j = 0; j <= 1; ++j) |
||||
for (int i = -1; i <= 1; ++i) |
||||
{ |
||||
if ((j == 0) && (i == -1)) |
||||
continue; |
||||
ContainerDepth delta_u = sqrt( |
||||
ContainerDepth(j) * ContainerDepth(j) + ContainerDepth(i) * ContainerDepth(i)); |
||||
ContainerDepth delta_z; |
||||
if (depth_in(y, x) > depth_in(y + j, x + i)) |
||||
delta_z = depth_in(y, x) - depth_in(y + j, x + i); |
||||
else |
||||
delta_z = depth_in(y + j, x + i) - depth_in(y, x); |
||||
if (delta_z < difference_threshold) |
||||
{ |
||||
delta_z *= scale; |
||||
ContainerDepth w = exp( |
||||
-delta_u * delta_u / 2 / sigma_L / sigma_L - delta_z * delta_z / 2 / sigma_z(y, x) / sigma_z(y, x)); |
||||
w_sum(y, x) += w; |
||||
Dw_sum(y, x) += depth_in(y + j, x + i) * w; |
||||
if ((j != 0) || (i != 0)) |
||||
{ |
||||
w = exp( |
||||
-delta_u * delta_u / 2 / sigma_L / sigma_L - delta_z * delta_z / 2 / sigma_z(y + j, x + i) |
||||
/ sigma_z(y + j, x + i)); |
||||
w_sum(y + j, x + i) += w; |
||||
Dw_sum(y + j, x + i) += depth_in(y, x) * w; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
cv::Mat(Dw_sum / w_sum).copyTo(depth_out); |
||||
} |
||||
}; |
||||
} |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace cv |
||||
{ |
||||
/** Default constructor of the Algorithm class that computes normals
|
||||
*/ |
||||
DepthCleaner::DepthCleaner(int depth, int window_size, int method_in) |
||||
: |
||||
depth_(depth), |
||||
window_size_(window_size), |
||||
method_(method_in), |
||||
depth_cleaner_impl_(0) |
||||
{ |
||||
CV_Assert(depth == CV_16U || depth == CV_32F || depth == CV_64F); |
||||
} |
||||
|
||||
/** Destructor
|
||||
*/ |
||||
DepthCleaner::~DepthCleaner() |
||||
{ |
||||
if (depth_cleaner_impl_ == 0) |
||||
return; |
||||
switch (method_) |
||||
{ |
||||
case DEPTH_CLEANER_NIL: |
||||
{ |
||||
switch (depth_) |
||||
{ |
||||
case CV_16U: |
||||
delete reinterpret_cast<const NIL<unsigned short> *>(depth_cleaner_impl_); |
||||
break; |
||||
case CV_32F: |
||||
delete reinterpret_cast<const NIL<float> *>(depth_cleaner_impl_); |
||||
break; |
||||
case CV_64F: |
||||
delete reinterpret_cast<const NIL<double> *>(depth_cleaner_impl_); |
||||
break; |
||||
} |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void |
||||
DepthCleaner::initialize_cleaner_impl() const |
||||
{ |
||||
CV_Assert(depth_ == CV_16U || depth_ == CV_32F || depth_ == CV_64F); |
||||
CV_Assert(window_size_ == 1 || window_size_ == 3 || window_size_ == 5 || window_size_ == 7); |
||||
CV_Assert( method_ == DEPTH_CLEANER_NIL); |
||||
switch (method_) |
||||
{ |
||||
case (DEPTH_CLEANER_NIL): |
||||
{ |
||||
switch (depth_) |
||||
{ |
||||
case CV_16U: |
||||
depth_cleaner_impl_ = new NIL<unsigned short>(window_size_, depth_, DEPTH_CLEANER_NIL); |
||||
break; |
||||
case CV_32F: |
||||
depth_cleaner_impl_ = new NIL<float>(window_size_, depth_, DEPTH_CLEANER_NIL); |
||||
break; |
||||
case CV_64F: |
||||
depth_cleaner_impl_ = new NIL<double>(window_size_, depth_, DEPTH_CLEANER_NIL); |
||||
break; |
||||
} |
||||
break; |
||||
} |
||||
} |
||||
|
||||
reinterpret_cast<DepthCleanerImpl *>(depth_cleaner_impl_)->cache(); |
||||
} |
||||
|
||||
/** Initializes some data that is cached for later computation
|
||||
* If that function is not called, it will be called the first time normals are computed |
||||
*/ |
||||
void |
||||
DepthCleaner::initialize() const |
||||
{ |
||||
if (depth_cleaner_impl_ == 0) |
||||
initialize_cleaner_impl(); |
||||
else if (!reinterpret_cast<DepthCleanerImpl *>(depth_cleaner_impl_)->validate(depth_, window_size_, method_)) |
||||
initialize_cleaner_impl(); |
||||
} |
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point
|
||||
* using the SRI method described in |
||||
* ``Fast and Accurate Computation of Surface Normals from Range Images`` |
||||
* by H. Badino, D. Huber, Y. Park and T. Kanade |
||||
* @param depth depth a float depth image. Or it can be rows x cols x 3 is they are 3d points |
||||
* @param window_size the window size on which to compute the derivatives |
||||
* @return normals a rows x cols x 3 matrix |
||||
*/ |
||||
void |
||||
DepthCleaner::operator()(InputArray depth_in_array, OutputArray depth_out_array) const |
||||
{ |
||||
cv::Mat depth_in = depth_in_array.getMat(); |
||||
CV_Assert(depth_in.dims == 2); |
||||
CV_Assert(depth_in.channels() == 1); |
||||
|
||||
depth_out_array.create(depth_in.size(), depth_); |
||||
cv::Mat depth_out = depth_out_array.getMat(); |
||||
|
||||
// Initialize the pimpl
|
||||
initialize(); |
||||
|
||||
// Clean the depth
|
||||
switch (method_) |
||||
{ |
||||
case (DEPTH_CLEANER_NIL): |
||||
{ |
||||
switch (depth_) |
||||
{ |
||||
case CV_16U: |
||||
reinterpret_cast<const NIL<unsigned short> *>(depth_cleaner_impl_)->compute(depth_in, depth_out); |
||||
break; |
||||
case CV_32F: |
||||
reinterpret_cast<const NIL<float> *>(depth_cleaner_impl_)->compute(depth_in, depth_out); |
||||
break; |
||||
case CV_64F: |
||||
reinterpret_cast<const NIL<double> *>(depth_cleaner_impl_)->compute(depth_in, depth_out); |
||||
break; |
||||
} |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -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 <opencv2/rgbd.hpp> |
||||
#include <limits> |
||||
|
||||
#include "depth_to_3d.h" |
||||
#include "utils.h" |
||||
|
||||
namespace |
||||
{ |
||||
/**
|
||||
* @param K |
||||
* @param depth the depth image |
||||
* @param mask the mask of the points to consider (can be empty) |
||||
* @param points3d the resulting 3d points, a 3-channel matrix |
||||
*/ |
||||
void |
||||
depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const cv::Mat& v_mat, const cv::Mat& z_mat, |
||||
cv::Mat& points3d) |
||||
{ |
||||
CV_Assert((u_mat.size() == z_mat.size()) && (v_mat.size() == z_mat.size())); |
||||
if (u_mat.empty()) |
||||
return; |
||||
CV_Assert((u_mat.type() == z_mat.type()) && (v_mat.type() == z_mat.type())); |
||||
|
||||
//grab camera params
|
||||
cv::Mat_<float> K; |
||||
|
||||
if (in_K.depth() == CV_32F) |
||||
K = in_K; |
||||
else |
||||
in_K.convertTo(K, CV_32F); |
||||
|
||||
float fx = K(0, 0); |
||||
float fy = K(1, 1); |
||||
float s = K(0, 1); |
||||
float cx = K(0, 2); |
||||
float cy = K(1, 2); |
||||
|
||||
std::vector<cv::Mat> coordinates(3); |
||||
|
||||
coordinates[0] = (u_mat - cx) / fx; |
||||
|
||||
if (s != 0) |
||||
coordinates[0] = coordinates[0] + (-(s / fy) * v_mat + cy * s / fy) / fx; |
||||
|
||||
coordinates[0] = coordinates[0].mul(z_mat); |
||||
coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy); |
||||
coordinates[2] = z_mat; |
||||
cv::merge(coordinates, points3d); |
||||
} |
||||
|
||||
/**
|
||||
* @param K |
||||
* @param depth the depth image |
||||
* @param mask the mask of the points to consider (can be empty) |
||||
* @param points3d the resulting 3d points |
||||
*/ |
||||
void |
||||
depthTo3dMask(const cv::Mat& depth, const cv::Mat& K, const cv::Mat& mask, cv::Mat& points3d) |
||||
{ |
||||
// Create 3D points in one go.
|
||||
cv::Mat_<float> u_mat, v_mat, z_mat; |
||||
|
||||
cv::Mat_<uchar> uchar_mask = mask; |
||||
|
||||
if (mask.depth() != (CV_8U)) |
||||
mask.convertTo(uchar_mask, CV_8U); |
||||
|
||||
// Figure out the interesting indices
|
||||
size_t n_points; |
||||
|
||||
if (depth.depth() == CV_16U) |
||||
n_points = convertDepthToFloat<uint16_t>(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat); |
||||
else if (depth.depth() == CV_16S) |
||||
n_points = convertDepthToFloat<int16_t>(depth, mask, 1.0 / 1000.0f, u_mat, v_mat, z_mat); |
||||
else |
||||
{ |
||||
CV_Assert(depth.type() == CV_32F); |
||||
n_points = convertDepthToFloat<float>(depth, mask, 1.0f, u_mat, v_mat, z_mat); |
||||
} |
||||
|
||||
if (n_points == 0) |
||||
return; |
||||
|
||||
u_mat.resize(n_points); |
||||
v_mat.resize(n_points); |
||||
z_mat.resize(n_points); |
||||
|
||||
depthTo3d_from_uvz(K, u_mat, v_mat, z_mat, points3d); |
||||
points3d = points3d.reshape(3, 1); |
||||
} |
||||
|
||||
/**
|
||||
* @param K |
||||
* @param depth the depth image |
||||
* @param points3d the resulting 3d points |
||||
*/ |
||||
template<typename T> |
||||
void |
||||
depthTo3dNoMask(const cv::Mat& in_depth, const cv::Mat_<T>& K, cv::Mat& points3d) |
||||
{ |
||||
const T inv_fx = T(1) / K(0, 0); |
||||
const T inv_fy = T(1) / K(1, 1); |
||||
const T ox = K(0, 2); |
||||
const T oy = K(1, 2); |
||||
|
||||
// Build z
|
||||
cv::Mat_<T> z_mat; |
||||
if (z_mat.depth() == in_depth.depth()) |
||||
z_mat = in_depth; |
||||
else |
||||
rescaleDepthTemplated<T>(in_depth, z_mat); |
||||
|
||||
// Pre-compute some constants
|
||||
cv::Mat_<T> x_cache(1, in_depth.cols), y_cache(in_depth.rows, 1); |
||||
T* x_cache_ptr = x_cache[0], *y_cache_ptr = y_cache[0]; |
||||
for (int x = 0; x < in_depth.cols; ++x, ++x_cache_ptr) |
||||
*x_cache_ptr = (x - ox) * inv_fx; |
||||
for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr) |
||||
*y_cache_ptr = (y - oy) * inv_fy; |
||||
|
||||
y_cache_ptr = y_cache[0]; |
||||
for (int y = 0; y < in_depth.rows; ++y, ++y_cache_ptr) |
||||
{ |
||||
cv::Vec<T, 3>* point = points3d.ptr<cv::Vec<T, 3> >(y); |
||||
const T* x_cache_ptr_end = x_cache[0] + in_depth.cols; |
||||
const T* depth = z_mat[y]; |
||||
for (x_cache_ptr = x_cache[0]; x_cache_ptr != x_cache_ptr_end; ++x_cache_ptr, ++point, ++depth) |
||||
{ |
||||
T z = *depth; |
||||
(*point)[0] = (*x_cache_ptr) * z; |
||||
(*point)[1] = (*y_cache_ptr) * z; |
||||
(*point)[2] = z; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace cv |
||||
{ |
||||
|
||||
/**
|
||||
* @param K |
||||
* @param depth the depth image |
||||
* @param u_mat the list of x coordinates |
||||
* @param v_mat the list of matching y coordinates |
||||
* @param points3d the resulting 3d points |
||||
*/ |
||||
void |
||||
depthTo3dSparse(InputArray depth_in, InputArray K_in, InputArray points_in, OutputArray points3d_out) |
||||
{ |
||||
// Make sure we use foat types
|
||||
cv::Mat points = points_in.getMat(); |
||||
cv::Mat depth = depth_in.getMat(); |
||||
|
||||
cv::Mat points_float; |
||||
if (points.depth() != CV_32F) |
||||
points.convertTo(points_float, CV_32FC2); |
||||
else |
||||
points_float = points; |
||||
|
||||
// Fill the depth matrix
|
||||
cv::Mat_<float> z_mat; |
||||
|
||||
if (depth.depth() == CV_16U) |
||||
convertDepthToFloat<uint16_t>(depth, 1.0 / 1000.0f, points_float, z_mat); |
||||
else if (depth.depth() == CV_16U) |
||||
convertDepthToFloat<int16_t>(depth, 1.0 / 1000.0f, points_float, z_mat); |
||||
else |
||||
{ |
||||
CV_Assert(depth.type() == CV_32F); |
||||
convertDepthToFloat<float>(depth, 1.0f, points_float, z_mat); |
||||
} |
||||
|
||||
std::vector<cv::Mat> channels(2); |
||||
cv::split(points_float, channels); |
||||
|
||||
points3d_out.create(channels[0].rows, channels[0].cols, CV_32FC3); |
||||
cv::Mat points3d = points3d_out.getMat(); |
||||
depthTo3d_from_uvz(K_in.getMat(), channels[0], channels[1], z_mat, points3d); |
||||
} |
||||
|
||||
/**
|
||||
* @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters |
||||
* (as done with the Microsoft Kinect), otherwise, if given as CV_32F, it is assumed in meters) |
||||
* @param K The calibration matrix |
||||
* @param points3d the resulting 3d points. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the |
||||
* depth of `K` if `depth` is of depth CV_U |
||||
* @param mask the mask of the points to consider (can be empty) |
||||
*/ |
||||
void |
||||
depthTo3d(InputArray depth_in, InputArray K_in, OutputArray points3d_out, InputArray mask_in) |
||||
{ |
||||
cv::Mat depth = depth_in.getMat(); |
||||
cv::Mat K = K_in.getMat(); |
||||
cv::Mat mask = mask_in.getMat(); |
||||
CV_Assert(K.cols == 3 && K.rows == 3 && (K.depth() == CV_64F || K.depth()==CV_32F)); |
||||
CV_Assert( |
||||
depth.type() == CV_64FC1 || depth.type() == CV_32FC1 || depth.type() == CV_16UC1 || depth.type() == CV_16SC1); |
||||
CV_Assert(mask.empty() || mask.channels() == 1); |
||||
|
||||
// TODO figure out what to do when types are different: convert or reject ?
|
||||
cv::Mat K_new; |
||||
if ((depth.depth() == CV_32F || depth.depth() == CV_64F) && depth.depth() != K.depth()) |
||||
{ |
||||
K.convertTo(K_new, depth.depth()); |
||||
} |
||||
else |
||||
K_new = K; |
||||
|
||||
// Create 3D points in one go.
|
||||
if (!mask.empty()) |
||||
{ |
||||
cv::Mat points3d; |
||||
depthTo3dMask(depth, K_new, mask, points3d); |
||||
points3d_out.create(points3d.size(), CV_MAKETYPE(K_new.depth(), 3)); |
||||
points3d.copyTo(points3d_out.getMat()); |
||||
} |
||||
else |
||||
{ |
||||
points3d_out.create(depth.size(), CV_MAKETYPE(K_new.depth(), 3)); |
||||
cv::Mat points3d = points3d_out.getMat(); |
||||
if (K_new.depth() == CV_64F) |
||||
depthTo3dNoMask<double>(depth, K_new, points3d); |
||||
else |
||||
depthTo3dNoMask<float>(depth, K_new, points3d); |
||||
} |
||||
} |
||||
} |
||||
@ -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 <opencv2/core.hpp> |
||||
#include <limits> |
||||
|
||||
/**
|
||||
* @param depth the depth image, containing depth with the value T |
||||
* @param the mask, containing CV_8UC1 |
||||
*/ |
||||
template <typename T> |
||||
size_t |
||||
convertDepthToFloat(const cv::Mat& depth, const cv::Mat& mask, float scale, cv::Mat_<float> &u_mat, cv::Mat_<float> &v_mat, cv::Mat_<float> &z_mat) |
||||
{ |
||||
CV_Assert (depth.size == mask.size); |
||||
|
||||
cv::Size depth_size = depth.size(); |
||||
|
||||
cv::Mat_<uchar> uchar_mask = mask; |
||||
|
||||
if (mask.depth() != CV_8U) |
||||
mask.convertTo(uchar_mask, CV_8U); |
||||
|
||||
u_mat = cv::Mat_<float>(depth_size.area(), 1); |
||||
v_mat = cv::Mat_<float>(depth_size.area(), 1); |
||||
z_mat = cv::Mat_<float>(depth_size.area(), 1); |
||||
|
||||
// Raw data from the Kinect has int
|
||||
size_t n_points = 0; |
||||
|
||||
for (int v = 0; v < depth_size.height; v++) |
||||
{ |
||||
uchar* r = uchar_mask.ptr<uchar>(v, 0); |
||||
|
||||
for (int u = 0; u < depth_size.width; u++, ++r) |
||||
if (*r) |
||||
{ |
||||
u_mat(n_points, 0) = u; |
||||
v_mat(n_points, 0) = v; |
||||
T depth_i = depth.at<T>(v, u); |
||||
|
||||
if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits<T>::min()) || (depth_i == std::numeric_limits<T>::max())) |
||||
z_mat(n_points, 0) = std::numeric_limits<float>::quiet_NaN(); |
||||
else |
||||
z_mat(n_points, 0) = depth_i * scale; |
||||
|
||||
++n_points; |
||||
} |
||||
} |
||||
|
||||
return n_points; |
||||
} |
||||
|
||||
/**
|
||||
* @param depth the depth image, containing depth with the value T |
||||
* @param the mask, containing CV_8UC1 |
||||
*/ |
||||
template <typename T> |
||||
void |
||||
convertDepthToFloat(const cv::Mat& depth, float scale, const cv::Mat &uv_mat, cv::Mat_<float> &z_mat) |
||||
{ |
||||
z_mat = cv::Mat_<float>(uv_mat.size()); |
||||
|
||||
// Raw data from the Kinect has int
|
||||
float* z_mat_iter = reinterpret_cast<float*>(z_mat.data); |
||||
|
||||
for (cv::Mat_<cv::Vec2f>::const_iterator uv_iter = uv_mat.begin<cv::Vec2f>(), uv_end = uv_mat.end<cv::Vec2f>(); |
||||
uv_iter != uv_end; ++uv_iter, ++z_mat_iter) |
||||
{ |
||||
T depth_i = depth.at < T > ((*uv_iter)[1], (*uv_iter)[0]); |
||||
|
||||
if (cvIsNaN(depth_i) || (depth_i == std::numeric_limits < T > ::min()) |
||||
|| (depth_i == std::numeric_limits < T > ::max())) |
||||
*z_mat_iter = std::numeric_limits<float>::quiet_NaN(); |
||||
else |
||||
*z_mat_iter = depth_i * scale; |
||||
} |
||||
} |
||||
|
||||
#endif /* __cplusplus */ |
||||
|
||||
#endif |
||||
|
||||
/* End of file. */ |
||||
@ -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 <opencv2/calib3d.hpp> |
||||
#include <opencv2/imgproc.hpp> |
||||
#include <opencv2/imgproc/types_c.h> |
||||
#include <opencv2/rgbd.hpp> |
||||
|
||||
namespace |
||||
{ |
||||
/** Just compute the norm of a vector
|
||||
* @param vec a vector of size 3 and any type T |
||||
* @return |
||||
*/ |
||||
template<typename T> |
||||
T |
||||
inline |
||||
norm_vec(const cv::Vec<T, 3> &vec) |
||||
{ |
||||
return std::sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]); |
||||
} |
||||
|
||||
/** Given 3d points, compute their distance to the origin
|
||||
* @param points |
||||
* @return |
||||
*/ |
||||
template<typename T> |
||||
cv::Mat_<T> |
||||
computeRadius(const cv::Mat &points) |
||||
{ |
||||
typedef cv::Vec<T, 3> PointT; |
||||
|
||||
// Compute the
|
||||
cv::Size size(points.cols, points.rows); |
||||
cv::Mat_<T> r(size); |
||||
if (points.isContinuous()) |
||||
size = cv::Size(points.cols * points.rows, 1); |
||||
for (int y = 0; y < size.height; ++y) |
||||
{ |
||||
const PointT* point = points.ptr < PointT > (y), *point_end = points.ptr < PointT > (y) + size.width; |
||||
T * row = r[y]; |
||||
for (; point != point_end; ++point, ++row) |
||||
*row = norm_vec(*point); |
||||
} |
||||
|
||||
return r; |
||||
} |
||||
|
||||
// Compute theta and phi according to equation 3 of
|
||||
// ``Fast and Accurate Computation of Surface Normals from Range Images``
|
||||
// by H. Badino, D. Huber, Y. Park and T. Kanade
|
||||
template<typename T> |
||||
void |
||||
computeThetaPhi(int rows, int cols, const cv::Matx<T, 3, 3>& K, cv::Mat &cos_theta, cv::Mat &sin_theta, |
||||
cv::Mat &cos_phi, cv::Mat &sin_phi) |
||||
{ |
||||
// Create some bogus coordinates
|
||||
cv::Mat depth_image = K(0, 0) * cv::Mat_ < T > ::ones(rows, cols); |
||||
cv::Mat points3d; |
||||
depthTo3d(depth_image, cv::Mat(K), points3d); |
||||
|
||||
typedef cv::Vec<T, 3> Vec3T; |
||||
|
||||
cos_theta = cv::Mat_ < T > (rows, cols); |
||||
sin_theta = cv::Mat_ < T > (rows, cols); |
||||
cos_phi = cv::Mat_ < T > (rows, cols); |
||||
sin_phi = cv::Mat_ < T > (rows, cols); |
||||
cv::Mat r = computeRadius<T>(points3d); |
||||
for (int y = 0; y < rows; ++y) |
||||
{ |
||||
T *row_cos_theta = cos_theta.ptr < T > (y), *row_sin_theta = sin_theta.ptr < T > (y); |
||||
T *row_cos_phi = cos_phi.ptr < T > (y), *row_sin_phi = sin_phi.ptr < T > (y); |
||||
const Vec3T * row_points = points3d.ptr < Vec3T > (y), *row_points_end = points3d.ptr < Vec3T |
||||
> (y) + points3d.cols; |
||||
const T * row_r = r.ptr < T > (y); |
||||
for (; row_points < row_points_end; |
||||
++row_cos_theta, ++row_sin_theta, ++row_cos_phi, ++row_sin_phi, ++row_points, ++row_r) |
||||
{ |
||||
// In the paper, z goes away from the camera, y goes down, x goes right
|
||||
// OpenCV has the same conventions
|
||||
// Theta goes from z to x (and actually goes from -pi/2 to pi/2, phi goes from z to y
|
||||
float theta = std::atan2(row_points->val[0], row_points->val[2]); |
||||
*row_cos_theta = std::cos(theta); |
||||
*row_sin_theta = std::sin(theta); |
||||
float phi = std::asin(row_points->val[1] / (*row_r)); |
||||
*row_cos_phi = std::cos(phi); |
||||
*row_sin_phi = std::sin(phi); |
||||
} |
||||
} |
||||
} |
||||
|
||||
/** Modify normals to make sure they point towards the camera
|
||||
* @param normals |
||||
*/ |
||||
template<typename T> |
||||
inline |
||||
void |
||||
signNormal(const cv::Vec<T, 3> & normal_in, cv::Vec<T, 3> & normal_out) |
||||
{ |
||||
cv::Vec<T, 3> res; |
||||
if (normal_in[2] > 0) |
||||
res = -normal_in / norm_vec(normal_in); |
||||
else |
||||
res = normal_in / norm_vec(normal_in); |
||||
normal_out[0] = res[0]; |
||||
normal_out[1] = res[1]; |
||||
normal_out[2] = res[2]; |
||||
} |
||||
/** Modify normals to make sure they point towards the camera
|
||||
* @param normals |
||||
*/ |
||||
template<typename T> |
||||
inline |
||||
void |
||||
signNormal(T a, T b, T c, cv::Vec<T, 3> & normal) |
||||
{ |
||||
T norm = 1 / std::sqrt(a * a + b * b + c * c); |
||||
if (c > 0) |
||||
{ |
||||
normal[0] = -a * norm; |
||||
normal[1] = -b * norm; |
||||
normal[2] = -c * norm; |
||||
} |
||||
else |
||||
{ |
||||
normal[0] = a * norm; |
||||
normal[1] = b * norm; |
||||
normal[2] = c * norm; |
||||
} |
||||
} |
||||
} |
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace |
||||
{ |
||||
class RgbdNormalsImpl |
||||
{ |
||||
public: |
||||
RgbdNormalsImpl(int rows, int cols, int window_size, int depth, const cv::Mat &K, |
||||
cv::RgbdNormals::RGBD_NORMALS_METHOD method) |
||||
: |
||||
rows_(rows), |
||||
cols_(cols), |
||||
depth_(depth), |
||||
window_size_(window_size), |
||||
method_(method) |
||||
{ |
||||
K.convertTo(K_, depth); |
||||
K.copyTo(K_ori_); |
||||
} |
||||
|
||||
virtual |
||||
~RgbdNormalsImpl() |
||||
{ |
||||
} |
||||
|
||||
virtual void |
||||
cache()=0; |
||||
|
||||
bool |
||||
validate(int rows, int cols, int depth, const cv::Mat &K_ori, int window_size, int method) const |
||||
{ |
||||
if ((K_ori.cols != K_ori_.cols) || (K_ori.rows != K_ori_.rows) || (K_ori.type() != K_ori_.type())) |
||||
return false; |
||||
bool K_test = !(cv::countNonZero(K_ori != K_ori_)); |
||||
return (rows == rows_) && (cols = cols_) && (window_size == window_size_) && (depth == depth_) && (K_test) |
||||
&& (method == method_); |
||||
} |
||||
protected: |
||||
int rows_, cols_, depth_; |
||||
cv::Mat K_, K_ori_; |
||||
int window_size_; |
||||
cv::RgbdNormals::RGBD_NORMALS_METHOD method_; |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point
|
||||
* using the FALS method described in |
||||
* ``Fast and Accurate Computation of Surface Normals from Range Images`` |
||||
* by H. Badino, D. Huber, Y. Park and T. Kanade |
||||
*/ |
||||
template<typename T> |
||||
class FALS: public RgbdNormalsImpl |
||||
{ |
||||
public: |
||||
typedef cv::Matx<T, 3, 3> Mat33T; |
||||
typedef cv::Vec<T, 9> Vec9T; |
||||
typedef cv::Vec<T, 3> Vec3T; |
||||
|
||||
FALS(int rows, int cols, int window_size, int depth, const cv::Mat &K, cv::RgbdNormals::RGBD_NORMALS_METHOD method) |
||||
: |
||||
RgbdNormalsImpl(rows, cols, window_size, depth, K, method) |
||||
{ |
||||
} |
||||
~FALS() |
||||
{ |
||||
} |
||||
|
||||
/** Compute cached data
|
||||
*/ |
||||
virtual void |
||||
cache() |
||||
{ |
||||
// Compute theta and phi according to equation 3
|
||||
cv::Mat cos_theta, sin_theta, cos_phi, sin_phi; |
||||
computeThetaPhi<T>(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi); |
||||
|
||||
// Compute all the v_i for every points
|
||||
std::vector<cv::Mat> channels(3); |
||||
channels[0] = sin_theta.mul(cos_phi); |
||||
channels[1] = sin_phi; |
||||
channels[2] = cos_theta.mul(cos_phi); |
||||
cv::merge(channels, V_); |
||||
|
||||
// Compute M
|
||||
cv::Mat_<Vec9T> M(rows_, cols_); |
||||
Mat33T VVt; |
||||
const Vec3T * vec = V_[0]; |
||||
Vec9T * M_ptr = M[0], *M_ptr_end = M_ptr + rows_ * cols_; |
||||
for (; M_ptr != M_ptr_end; ++vec, ++M_ptr) |
||||
{ |
||||
VVt = (*vec) * vec->t(); |
||||
*M_ptr = Vec9T(VVt.val); |
||||
} |
||||
|
||||
cv::boxFilter(M, M, M.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); |
||||
|
||||
// Compute M's inverse
|
||||
Mat33T M_inv; |
||||
M_inv_.create(rows_, cols_); |
||||
Vec9T * M_inv_ptr = M_inv_[0]; |
||||
for (M_ptr = &M(0); M_ptr != M_ptr_end; ++M_inv_ptr, ++M_ptr) |
||||
{ |
||||
// We have a semi-definite matrix
|
||||
cv::invert(Mat33T(M_ptr->val), M_inv, cv::DECOMP_CHOLESKY); |
||||
*M_inv_ptr = Vec9T(M_inv.val); |
||||
} |
||||
} |
||||
|
||||
/** Compute the normals
|
||||
* @param r |
||||
* @return |
||||
*/ |
||||
virtual void |
||||
compute(const cv::Mat&, const cv::Mat &r, cv::Mat & normals) const |
||||
{ |
||||
// Compute B
|
||||
cv::Mat_<Vec3T> B(rows_, cols_); |
||||
|
||||
const T* row_r = r.ptr < T > (0), *row_r_end = row_r + rows_ * cols_; |
||||
const Vec3T *row_V = V_[0]; |
||||
Vec3T *row_B = B[0]; |
||||
for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V) |
||||
{ |
||||
if (cvIsNaN(*row_r)) |
||||
*row_B = Vec3T(); |
||||
else |
||||
*row_B = (*row_V) / (*row_r); |
||||
} |
||||
|
||||
// Apply a box filter to B
|
||||
cv::boxFilter(B, B, B.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); |
||||
|
||||
// compute the Minv*B products
|
||||
row_r = r.ptr < T > (0); |
||||
const Vec3T * B_vec = B[0]; |
||||
const Mat33T * M_inv = reinterpret_cast<const Mat33T *>(M_inv_.ptr(0)); |
||||
Vec3T *normal = normals.ptr<Vec3T>(0); |
||||
for (; row_r != row_r_end; ++row_r, ++B_vec, ++normal, ++M_inv) |
||||
if (cvIsNaN(*row_r)) |
||||
{ |
||||
(*normal)[0] = *row_r; |
||||
(*normal)[1] = *row_r; |
||||
(*normal)[2] = *row_r; |
||||
} |
||||
else |
||||
signNormal((*M_inv) * (*B_vec), *normal); |
||||
} |
||||
|
||||
private: |
||||
cv::Mat_<Vec3T> V_; |
||||
cv::Mat_<Vec9T> M_inv_; |
||||
}; |
||||
} |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** Function that multiplies K_inv by a vector. It is just meant to speed up the product as we know
|
||||
* that K_inv is upper triangular and K_inv(2,2)=1 |
||||
* @param K_inv |
||||
* @param a |
||||
* @param b |
||||
* @param c |
||||
* @param res |
||||
*/ |
||||
template<typename T, typename U> |
||||
void |
||||
multiply_by_K_inv(const cv::Matx<T, 3, 3> & K_inv, U a, U b, U c, cv::Vec<T, 3> &res) |
||||
{ |
||||
res[0] = K_inv(0, 0) * a + K_inv(0, 1) * b + K_inv(0, 2) * c; |
||||
res[1] = K_inv(1, 1) * b + K_inv(1, 2) * c; |
||||
res[2] = c; |
||||
} |
||||
|
||||
namespace |
||||
{ |
||||
/** Given a depth image, compute the normals as detailed in the LINEMOD paper
|
||||
* ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects`` |
||||
* by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit |
||||
*/ |
||||
template<typename T> |
||||
class LINEMOD: public RgbdNormalsImpl |
||||
{ |
||||
public: |
||||
typedef cv::Vec<T, 3> Vec3T; |
||||
typedef cv::Matx<T, 3, 3> Mat33T; |
||||
|
||||
LINEMOD(int rows, int cols, int window_size, int depth, const cv::Mat &K, |
||||
cv::RgbdNormals::RGBD_NORMALS_METHOD method) |
||||
: |
||||
RgbdNormalsImpl(rows, cols, window_size, depth, K, method) |
||||
{ |
||||
} |
||||
|
||||
/** Compute cached data
|
||||
*/ |
||||
virtual void |
||||
cache() |
||||
{ |
||||
} |
||||
|
||||
/** Compute the normals
|
||||
* @param r |
||||
* @param normals the output normals |
||||
*/ |
||||
void |
||||
compute(const cv::Mat& depth_in, cv::Mat & normals) const |
||||
{ |
||||
switch (depth_in.depth()) |
||||
{ |
||||
case CV_16U: |
||||
{ |
||||
const cv::Mat_<unsigned short> &depth(depth_in); |
||||
computeImpl<unsigned short, long>(depth, normals); |
||||
break; |
||||
} |
||||
case CV_32F: |
||||
{ |
||||
const cv::Mat_<float> &depth(depth_in); |
||||
computeImpl<float, float>(depth, normals); |
||||
break; |
||||
} |
||||
case CV_64F: |
||||
{ |
||||
const cv::Mat_<double> &depth(depth_in); |
||||
computeImpl<double, double>(depth, normals); |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
private: |
||||
/** Compute the normals
|
||||
* @param r |
||||
* @return |
||||
*/ |
||||
template<typename DepthDepth, typename ContainerDepth> |
||||
cv::Mat |
||||
computeImpl(const cv::Mat_<DepthDepth> &depth, cv::Mat & normals) const |
||||
{ |
||||
const int r = 5; // used to be 7
|
||||
const int sample_step = r; |
||||
const int square_size = ((2 * r / sample_step) + 1); |
||||
long offsets[square_size * square_size]; |
||||
long offsets_x[square_size * square_size]; |
||||
long offsets_y[square_size * square_size]; |
||||
long offsets_x_x[square_size * square_size]; |
||||
long offsets_x_y[square_size * square_size]; |
||||
long offsets_y_y[square_size * square_size]; |
||||
for (int j = -r, index = 0; j <= r; j += sample_step) |
||||
for (int i = -r; i <= r; i += sample_step, ++index) |
||||
{ |
||||
offsets_x[index] = i; |
||||
offsets_y[index] = j; |
||||
offsets_x_x[index] = i*i; |
||||
offsets_x_y[index] = i*j; |
||||
offsets_y_y[index] = j*j; |
||||
offsets[index] = j * cols_ + i; |
||||
} |
||||
|
||||
// Define K_inv by hand, just for higher accuracy
|
||||
Mat33T K_inv = cv::Matx<T, 3, 3>::eye(), K; |
||||
K_.copyTo(K); |
||||
K_inv(0, 0) = 1.0 / K(0, 0); |
||||
K_inv(0, 1) = -K(0, 1) / (K(0, 0) * K(1, 1)); |
||||
K_inv(0, 2) = (K(0, 1) * K(1, 2) - K(0, 2) * K(1, 1)) / (K(0, 0) * K(1, 1)); |
||||
K_inv(1, 1) = 1 / K(1, 1); |
||||
K_inv(1, 2) = -K(1, 2) / K(1, 1); |
||||
|
||||
Vec3T X1_minus_X, X2_minus_X; |
||||
|
||||
ContainerDepth difference_threshold = 50; |
||||
for (int y = r; y < rows_ - r - 1; ++y) |
||||
{ |
||||
const DepthDepth * p_line = reinterpret_cast<const DepthDepth*>(depth.ptr(y, r)); |
||||
Vec3T *normal = normals.ptr<Vec3T>(y, r); |
||||
|
||||
for (int x = r; x < cols_ - r - 1; ++x) |
||||
{ |
||||
DepthDepth d = p_line[0]; |
||||
|
||||
// accum
|
||||
long A[4]; |
||||
A[0] = A[1] = A[2] = A[3] = 0; |
||||
ContainerDepth b[2]; |
||||
b[0] = b[1] = 0; |
||||
for (unsigned int i = 0; i < square_size * square_size; ++i) { |
||||
// We need to cast to ContainerDepth in case we have unsigned DepthDepth
|
||||
ContainerDepth delta = ContainerDepth(p_line[offsets[i]]) - ContainerDepth(d); |
||||
if (std::abs(delta) > difference_threshold) |
||||
continue; |
||||
|
||||
A[0] += offsets_x_x[i]; |
||||
A[1] += offsets_x_y[i]; |
||||
A[3] += offsets_y_y[i]; |
||||
b[0] += offsets_x[i] * delta; |
||||
b[1] += offsets_y[i] * delta; |
||||
} |
||||
|
||||
// solve for the optimal gradient D of equation (8)
|
||||
long det = A[0] * A[3] - A[1] * A[1]; |
||||
// We should divide the following two by det, but instead, we multiply
|
||||
// X1_minus_X and X2_minus_X by det (which does not matter as we normalize the normals)
|
||||
// Therefore, no division is done: this is only for speedup
|
||||
ContainerDepth dx = (A[3] * b[0] - A[1] * b[1]); |
||||
ContainerDepth dy = (-A[1] * b[0] + A[0] * b[1]); |
||||
|
||||
// Compute the dot product
|
||||
//Vec3T X = K_inv * Vec3T(x, y, 1) * depth(y, x);
|
||||
//Vec3T X1 = K_inv * Vec3T(x + 1, y, 1) * (depth(y, x) + dx);
|
||||
//Vec3T X2 = K_inv * Vec3T(x, y + 1, 1) * (depth(y, x) + dy);
|
||||
//Vec3T nor = (X1 - X).cross(X2 - X);
|
||||
multiply_by_K_inv(K_inv, d * det + (x + 1) * dx, y * dx, dx, X1_minus_X); |
||||
multiply_by_K_inv(K_inv, x * dy, d * det + (y + 1) * dy, dy, X2_minus_X); |
||||
Vec3T nor = X1_minus_X.cross(X2_minus_X); |
||||
signNormal(nor, *normal); |
||||
|
||||
++p_line; |
||||
++normal; |
||||
} |
||||
} |
||||
|
||||
return normals; |
||||
} |
||||
}; |
||||
} |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace |
||||
{ |
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point
|
||||
* using the SRI method described in |
||||
* ``Fast and Accurate Computation of Surface Normals from Range Images`` |
||||
* by H. Badino, D. Huber, Y. Park and T. Kanade |
||||
*/ |
||||
template<typename T> |
||||
class SRI: public RgbdNormalsImpl |
||||
{ |
||||
public: |
||||
typedef cv::Matx<T, 3, 3> Mat33T; |
||||
typedef cv::Vec<T, 9> Vec9T; |
||||
typedef cv::Vec<T, 3> Vec3T; |
||||
|
||||
SRI(int rows, int cols, int window_size, int depth, const cv::Mat &K, cv::RgbdNormals::RGBD_NORMALS_METHOD method) |
||||
: |
||||
RgbdNormalsImpl(rows, cols, window_size, depth, K, method), |
||||
phi_step_(0), |
||||
theta_step_(0) |
||||
{ |
||||
} |
||||
|
||||
/** Compute cached data
|
||||
*/ |
||||
virtual void |
||||
cache() |
||||
{ |
||||
cv::Mat_<T> cos_theta, sin_theta, cos_phi, sin_phi; |
||||
computeThetaPhi<T>(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi); |
||||
|
||||
// Create the derivative kernels
|
||||
getDerivKernels(kx_dx_, ky_dx_, 1, 0, window_size_, true, depth_); |
||||
getDerivKernels(kx_dy_, ky_dy_, 0, 1, window_size_, true, depth_); |
||||
|
||||
// Get the mapping function for SRI
|
||||
float min_theta = std::asin(sin_theta(0, 0)), max_theta = std::asin(sin_theta(0, cols_ - 1)); |
||||
float min_phi = std::asin(sin_phi(0, cols_/2-1)), max_phi = std::asin(sin_phi(rows_ - 1, cols_/2-1)); |
||||
|
||||
std::vector<cv::Point3f> points3d(cols_ * rows_); |
||||
R_hat_.create(rows_, cols_); |
||||
phi_step_ = float(max_phi - min_phi) / (rows_ - 1); |
||||
theta_step_ = float(max_theta - min_theta) / (cols_ - 1); |
||||
for (int phi_int = 0, k = 0; phi_int < rows_; ++phi_int) |
||||
{ |
||||
float phi = min_phi + phi_int * phi_step_; |
||||
for (int theta_int = 0; theta_int < cols_; ++theta_int, ++k) |
||||
{ |
||||
float theta = min_theta + theta_int * theta_step_; |
||||
// Store the 3d point to project it later
|
||||
points3d[k] = cv::Point3f(std::sin(theta) * std::cos(phi), std::sin(phi), std::cos(theta) * std::cos(phi)); |
||||
|
||||
// Cache the rotation matrix and negate it
|
||||
cv::Mat_<T> mat = |
||||
(cv::Mat_ < T > (3, 3) << 0, 1, 0, 0, 0, 1, 1, 0, 0) * ((cv::Mat_ < T > (3, 3) << std::cos(theta), -std::sin( |
||||
theta), 0, std::sin(theta), std::cos(theta), 0, 0, 0, 1)) |
||||
* ((cv::Mat_ < T > (3, 3) << std::cos(phi), 0, -std::sin(phi), 0, 1, 0, std::sin(phi), 0, std::cos(phi))); |
||||
for (unsigned char i = 0; i < 3; ++i) |
||||
mat(i, 1) = mat(i, 1) / std::cos(phi); |
||||
// The second part of the matrix is never explained in the paper ... but look at the wikipedia normal article
|
||||
mat(0, 0) = mat(0, 0) - 2 * std::cos(phi) * std::sin(theta); |
||||
mat(1, 0) = mat(1, 0) - 2 * std::sin(phi); |
||||
mat(2, 0) = mat(2, 0) - 2 * std::cos(phi) * std::cos(theta); |
||||
|
||||
R_hat_(phi_int, theta_int) = Vec9T((T*) (mat.data)); |
||||
} |
||||
} |
||||
|
||||
map_.create(rows_, cols_); |
||||
cv::projectPoints(points3d, cv::Mat(3,1,CV_32FC1,cv::Scalar::all(0.0f)), cv::Mat(3,1,CV_32FC1,cv::Scalar::all(0.0f)), K_, cv::Mat(), map_); |
||||
map_ = map_.reshape(2, rows_); |
||||
cv::convertMaps(map_, cv::Mat(), xy_, fxy_, CV_16SC2); |
||||
|
||||
//map for converting from Spherical coordinate space to Euclidean space
|
||||
euclideanMap_.create(rows_,cols_); |
||||
float invFx = 1.0f/K_.at<T>(0,0), cx = K_.at<T>(0,2); |
||||
double invFy = 1.0f/K_.at<T>(1,1), cy = K_.at<T>(1,2); |
||||
for (int i = 0; i < rows_; i++) |
||||
{ |
||||
float y = (i - cy)*invFy; |
||||
for (int j = 0; j < cols_; j++) |
||||
{ |
||||
float x = (j - cx)*invFx; |
||||
float theta = std::atan(x); |
||||
float phi = std::asin(y/std::sqrt(x*x+y*y+1.0f)); |
||||
|
||||
euclideanMap_(i,j) = cv::Vec2f((theta-min_theta)/theta_step_,(phi-min_phi)/phi_step_); |
||||
} |
||||
} |
||||
//convert map to 2 maps in short format for increasing speed in remap function
|
||||
cv::convertMaps(euclideanMap_, cv::Mat(), invxy_, invfxy_, CV_16SC2); |
||||
|
||||
// Update the kernels: the steps are due to the fact that derivatives will be computed on a grid where
|
||||
// the step is not 1. Only need to do it on one dimension as it computes derivatives in only one direction
|
||||
kx_dx_ /= theta_step_; |
||||
ky_dy_ /= phi_step_; |
||||
} |
||||
|
||||
/** Compute the normals
|
||||
* @param r |
||||
* @return |
||||
*/ |
||||
virtual void |
||||
compute(const cv::Mat& points3d, const cv::Mat &r, cv::Mat & normals) const |
||||
{ |
||||
const cv::Mat_<T>& r_T(r); |
||||
const cv::Mat_<Vec3T> &points3d_T(points3d); |
||||
compute(points3d_T, r_T, normals); |
||||
} |
||||
|
||||
/** Compute the normals
|
||||
* @param r |
||||
* @return |
||||
*/ |
||||
void |
||||
compute(const cv::Mat_<Vec3T> &, const cv::Mat_<T> &r_non_interp, cv::Mat & normals_out) const |
||||
{ |
||||
// Interpolate the radial image to make derivatives meaningful
|
||||
cv::Mat_<T> r; |
||||
// higher quality remapping does not help here
|
||||
cv::remap(r_non_interp, r, xy_, fxy_, CV_INTER_LINEAR); |
||||
|
||||
// Compute the derivatives with respect to theta and phi
|
||||
// TODO add bilateral filtering (as done in kinfu)
|
||||
cv::Mat_<T> r_theta, r_phi; |
||||
cv::sepFilter2D(r, r_theta, r.depth(), kx_dx_, ky_dx_); |
||||
//current OpenCV version sometimes corrupts r matrix after second call of sepFilter2D
|
||||
//it depends on resolution, be careful
|
||||
cv::sepFilter2D(r, r_phi, r.depth(), kx_dy_, ky_dy_); |
||||
|
||||
// Fill the result matrix
|
||||
cv::Mat_<Vec3T> normals(rows_, cols_); |
||||
|
||||
const T* r_theta_ptr = r_theta[0], *r_theta_ptr_end = r_theta_ptr + rows_ * cols_; |
||||
const T* r_phi_ptr = r_phi[0]; |
||||
const Mat33T * R = reinterpret_cast<const Mat33T *>(R_hat_[0]); |
||||
const T* r_ptr = r[0]; |
||||
Vec3T * normal = normals[0]; |
||||
for (; r_theta_ptr != r_theta_ptr_end; ++r_theta_ptr, ++r_phi_ptr, ++R, ++r_ptr, ++normal) |
||||
{ |
||||
if (cvIsNaN(*r_ptr)) |
||||
{ |
||||
(*normal)[0] = *r_ptr; |
||||
(*normal)[1] = *r_ptr; |
||||
(*normal)[2] = *r_ptr; |
||||
} |
||||
else |
||||
{ |
||||
T r_theta_over_r = (*r_theta_ptr) / (*r_ptr); |
||||
T r_phi_over_r = (*r_phi_ptr) / (*r_ptr); |
||||
// R(1,1) is 0
|
||||
signNormal((*R)(0, 0) + (*R)(0, 1) * r_theta_over_r + (*R)(0, 2) * r_phi_over_r, |
||||
(*R)(1, 0) + (*R)(1, 2) * r_phi_over_r, |
||||
(*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal); |
||||
} |
||||
} |
||||
|
||||
cv::remap(normals, normals_out, invxy_, invfxy_, cv::INTER_LINEAR); |
||||
normal = normals_out.ptr<Vec3T>(0); |
||||
Vec3T * normal_end = normal + rows_ * cols_; |
||||
for (; normal != normal_end; ++normal) |
||||
signNormal((*normal)[0], (*normal)[1], (*normal)[2], *normal); |
||||
} |
||||
private: |
||||
/** Stores R */ |
||||
cv::Mat_<Vec9T> R_hat_; |
||||
float phi_step_, theta_step_; |
||||
|
||||
/** Derivative kernels */ |
||||
cv::Mat kx_dx_, ky_dx_, kx_dy_, ky_dy_; |
||||
/** mapping function to get an SRI image */ |
||||
cv::Mat_<cv::Vec2f> map_; |
||||
cv::Mat xy_, fxy_; |
||||
|
||||
cv::Mat_<cv::Vec2f> euclideanMap_; |
||||
cv::Mat invxy_, invfxy_; |
||||
}; |
||||
} |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace cv |
||||
{ |
||||
/** Default constructor of the Algorithm class that computes normals
|
||||
*/ |
||||
RgbdNormals::RgbdNormals(int rows, int cols, int depth, InputArray K_in, int window_size, int method_in) |
||||
: |
||||
rows_(rows), |
||||
cols_(cols), |
||||
depth_(depth), |
||||
K_(K_in.getMat()), |
||||
window_size_(window_size), |
||||
method_(method_in), |
||||
rgbd_normals_impl_(0) |
||||
{ |
||||
CV_Assert(depth == CV_32F || depth == CV_64F); |
||||
CV_Assert(K_.cols == 3 && K_.rows == 3); |
||||
} |
||||
|
||||
// Just to remove a warning
|
||||
void delete_normals_impl(void *rgbd_normals_impl_, int method_, int depth); |
||||
void delete_normals_impl(void *rgbd_normals_impl_, int method_, int depth) |
||||
{ |
||||
if (rgbd_normals_impl_ == 0) |
||||
return; |
||||
switch (method_) |
||||
{ |
||||
case RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD: |
||||
{ |
||||
if (depth == CV_32F) |
||||
delete reinterpret_cast<const LINEMOD<float> *>(rgbd_normals_impl_); |
||||
else |
||||
delete reinterpret_cast<const LINEMOD<double> *>(rgbd_normals_impl_); |
||||
break; |
||||
} |
||||
case RgbdNormals::RGBD_NORMALS_METHOD_SRI: |
||||
{ |
||||
if (depth == CV_32F) |
||||
delete reinterpret_cast<const SRI<float> *>(rgbd_normals_impl_); |
||||
else |
||||
delete reinterpret_cast<const SRI<double> *>(rgbd_normals_impl_); |
||||
break; |
||||
} |
||||
case (RgbdNormals::RGBD_NORMALS_METHOD_FALS): |
||||
{ |
||||
if (depth == CV_32F) |
||||
delete reinterpret_cast<const FALS<float> *>(rgbd_normals_impl_); |
||||
else |
||||
delete reinterpret_cast<const FALS<double> *>(rgbd_normals_impl_); |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
/** Destructor
|
||||
*/ |
||||
RgbdNormals::~RgbdNormals() |
||||
{ |
||||
delete_normals_impl(rgbd_normals_impl_, method_, depth_); |
||||
} |
||||
|
||||
void |
||||
RgbdNormals::initialize_normals_impl(int rows, int cols, int depth, const cv::Mat & K, int window_size, |
||||
int method_in) const |
||||
{ |
||||
CV_Assert(rows > 0 && cols > 0 && (depth == CV_32F || depth == CV_64F)); |
||||
CV_Assert(window_size == 1 || window_size == 3 || window_size == 5 || window_size == 7); |
||||
CV_Assert(K_.cols == 3 && K.rows == 3 && (K.depth() == CV_32F || K.depth() == CV_64F)); |
||||
CV_Assert( |
||||
method_in == RGBD_NORMALS_METHOD_FALS || method_in == RGBD_NORMALS_METHOD_LINEMOD |
||||
|| method_in == RGBD_NORMALS_METHOD_SRI); |
||||
switch (method_in) |
||||
{ |
||||
case (RGBD_NORMALS_METHOD_FALS): |
||||
{ |
||||
if (depth == CV_32F) |
||||
rgbd_normals_impl_ = new FALS<float>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_FALS); |
||||
else |
||||
rgbd_normals_impl_ = new FALS<double>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_FALS); |
||||
break; |
||||
} |
||||
case (RGBD_NORMALS_METHOD_LINEMOD): |
||||
{ |
||||
if (depth == CV_32F) |
||||
rgbd_normals_impl_ = new LINEMOD<float>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_LINEMOD); |
||||
else |
||||
rgbd_normals_impl_ = new LINEMOD<double>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_LINEMOD); |
||||
break; |
||||
} |
||||
case RGBD_NORMALS_METHOD_SRI: |
||||
{ |
||||
if (depth == CV_32F) |
||||
rgbd_normals_impl_ = new SRI<float>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_SRI); |
||||
else |
||||
rgbd_normals_impl_ = new SRI<double>(rows, cols, window_size, depth, K, RGBD_NORMALS_METHOD_SRI); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
reinterpret_cast<RgbdNormalsImpl *>(rgbd_normals_impl_)->cache(); |
||||
} |
||||
|
||||
/** Initializes some data that is cached for later computation
|
||||
* If that function is not called, it will be called the first time normals are computed |
||||
*/ |
||||
void |
||||
RgbdNormals::initialize() const |
||||
{ |
||||
if (rgbd_normals_impl_ == 0) |
||||
initialize_normals_impl(rows_, cols_, depth_, K_, window_size_, method_); |
||||
else if (!reinterpret_cast<RgbdNormalsImpl *>(rgbd_normals_impl_)->validate(rows_, cols_, depth_, K_, window_size_, |
||||
method_)) { |
||||
delete_normals_impl(rgbd_normals_impl_, method_, depth_); |
||||
initialize_normals_impl(rows_, cols_, depth_, K_, window_size_, method_); |
||||
} |
||||
} |
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point
|
||||
* @param points3d_in depth a float depth image. Or it can be rows x cols x 3 is they are 3d points |
||||
* @param normals a rows x cols x 3 matrix |
||||
*/ |
||||
void |
||||
RgbdNormals::operator()(InputArray points3d_in, OutputArray normals_out) const |
||||
{ |
||||
cv::Mat points3d_ori = points3d_in.getMat(); |
||||
|
||||
CV_Assert(points3d_ori.dims == 2); |
||||
// Either we have 3d points or a depth image
|
||||
switch (method_) |
||||
{ |
||||
case (RGBD_NORMALS_METHOD_FALS): |
||||
{ |
||||
CV_Assert(points3d_ori.channels() == 3); |
||||
CV_Assert(points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F); |
||||
break; |
||||
} |
||||
case RGBD_NORMALS_METHOD_LINEMOD: |
||||
{ |
||||
CV_Assert( |
||||
((points3d_ori.channels() == 3) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)) || ((points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F))); |
||||
break; |
||||
} |
||||
case RGBD_NORMALS_METHOD_SRI: |
||||
{ |
||||
CV_Assert( ((points3d_ori.channels() == 3) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F))); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
// Initialize the pimpl
|
||||
initialize(); |
||||
|
||||
// Precompute something for RGBD_NORMALS_METHOD_SRI and RGBD_NORMALS_METHOD_FALS
|
||||
cv::Mat points3d, radius; |
||||
if ((method_ == RGBD_NORMALS_METHOD_SRI) || (method_ == RGBD_NORMALS_METHOD_FALS)) |
||||
{ |
||||
// Make the points have the right depth
|
||||
if (points3d_ori.depth() == depth_) |
||||
points3d = points3d_ori; |
||||
else |
||||
points3d_ori.convertTo(points3d, depth_); |
||||
|
||||
// Compute the distance to the points
|
||||
if (depth_ == CV_32F) |
||||
radius = computeRadius<float>(points3d); |
||||
else |
||||
radius = computeRadius<double>(points3d); |
||||
} |
||||
|
||||
// Get the normals
|
||||
normals_out.create(points3d_ori.size(), CV_MAKETYPE(depth_, 3)); |
||||
if (points3d_in.empty()) |
||||
return; |
||||
|
||||
cv::Mat normals = normals_out.getMat(); |
||||
switch (method_) |
||||
{ |
||||
case (RGBD_NORMALS_METHOD_FALS): |
||||
{ |
||||
if (depth_ == CV_32F) |
||||
reinterpret_cast<const FALS<float> *>(rgbd_normals_impl_)->compute(points3d, radius, normals); |
||||
else |
||||
reinterpret_cast<const FALS<double> *>(rgbd_normals_impl_)->compute(points3d, radius, normals); |
||||
break; |
||||
} |
||||
case RGBD_NORMALS_METHOD_LINEMOD: |
||||
{ |
||||
// Only focus on the depth image for LINEMOD
|
||||
cv::Mat depth; |
||||
if (points3d_ori.channels() == 3) |
||||
{ |
||||
std::vector<cv::Mat> channels; |
||||
cv::split(points3d, channels); |
||||
depth = channels[2]; |
||||
} |
||||
else |
||||
depth = points3d_ori; |
||||
|
||||
if (depth_ == CV_32F) |
||||
reinterpret_cast<const LINEMOD<float> *>(rgbd_normals_impl_)->compute(depth, normals); |
||||
else |
||||
reinterpret_cast<const LINEMOD<double> *>(rgbd_normals_impl_)->compute(depth, normals); |
||||
break; |
||||
} |
||||
case RGBD_NORMALS_METHOD_SRI: |
||||
{ |
||||
if (depth_ == CV_32F) |
||||
reinterpret_cast<const SRI<float> *>(rgbd_normals_impl_)->compute(points3d, radius, normals); |
||||
else |
||||
reinterpret_cast<const SRI<double> *>(rgbd_normals_impl_)->compute(points3d, radius, normals); |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
File diff suppressed because it is too large
Load Diff
@ -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 <list> |
||||
#include <set> |
||||
|
||||
#include <opencv2/rgbd.hpp> |
||||
|
||||
/** Structure defining a plane. The notations are from the second paper */ |
||||
class PlaneBase |
||||
{ |
||||
public: |
||||
PlaneBase(const cv::Vec3f & m, const cv::Vec3f &n_in, int index) |
||||
: |
||||
index_(index), |
||||
n_(n_in), |
||||
m_sum_(cv::Vec3f(0, 0, 0)), |
||||
m_(m), |
||||
Q_(cv::Matx33f::zeros()), |
||||
mse_(0), |
||||
K_(0) |
||||
{ |
||||
UpdateD(); |
||||
} |
||||
|
||||
virtual |
||||
~PlaneBase() |
||||
{ |
||||
} |
||||
|
||||
/** Compute the distance to the plane. This will be implemented by the children to take into account different
|
||||
* sensor models |
||||
* @param p_j |
||||
* @return |
||||
*/ |
||||
virtual |
||||
float |
||||
distance(const cv::Vec3f& p_j) const = 0; |
||||
|
||||
/** The d coefficient in the plane equation ax+by+cz+d = 0
|
||||
* @return |
||||
*/ |
||||
inline float |
||||
d() const |
||||
{ |
||||
return d_; |
||||
} |
||||
|
||||
/** The normal to the plane
|
||||
* @return the normal to the plane |
||||
*/ |
||||
const cv::Vec3f & |
||||
n() const |
||||
{ |
||||
return n_; |
||||
} |
||||
|
||||
/** Update the different coefficients of the plane, based on the new statistics
|
||||
*/ |
||||
void |
||||
UpdateParameters() |
||||
{ |
||||
if (empty()) |
||||
return; |
||||
m_ = m_sum_ / K_; |
||||
// Compute C
|
||||
cv::Matx33f C = Q_ - m_sum_ * m_.t(); |
||||
|
||||
// Compute n
|
||||
cv::SVD svd(C); |
||||
n_ = cv::Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2)); |
||||
mse_ = svd.w.at<float>(2) / K_; |
||||
|
||||
UpdateD(); |
||||
} |
||||
|
||||
/** Update the different sum of point and sum of point*point.t()
|
||||
*/ |
||||
void |
||||
UpdateStatistics(const cv::Vec3f & point, const cv::Matx33f & Q_local) |
||||
{ |
||||
m_sum_ += point; |
||||
Q_ += Q_local; |
||||
++K_; |
||||
} |
||||
|
||||
inline size_t |
||||
empty() const |
||||
{ |
||||
return K_ == 0; |
||||
} |
||||
|
||||
inline int |
||||
K() const |
||||
{ |
||||
return K_; |
||||
} |
||||
/** The index of the plane */ |
||||
int index_; |
||||
protected: |
||||
/** The 4th coefficient in the plane equation ax+by+cz+d = 0 */ |
||||
float d_; |
||||
/** Normal of the plane */ |
||||
cv::Vec3f n_; |
||||
private: |
||||
inline void |
||||
UpdateD() |
||||
{ |
||||
d_ = -m_.dot(n_); |
||||
} |
||||
/** The sum of the points */ |
||||
cv::Vec3f m_sum_; |
||||
/** The mean of the points */ |
||||
cv::Vec3f m_; |
||||
/** The sum of pi * pi^\top */ |
||||
cv::Matx33f Q_; |
||||
/** The different matrices we need to update */ |
||||
cv::Matx33f C_; |
||||
float mse_; |
||||
/** the number of points that form the plane */ |
||||
int K_; |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** Basic planar child, with no sensor error model
|
||||
*/ |
||||
class Plane: public PlaneBase |
||||
{ |
||||
public: |
||||
Plane(const cv::Vec3f & m, const cv::Vec3f &n_in, int index) |
||||
: |
||||
PlaneBase(m, n_in, index) |
||||
{ |
||||
} |
||||
|
||||
/** The computed distance is perfect in that case
|
||||
* @param p_j the point to compute its distance to |
||||
* @return |
||||
*/ |
||||
float |
||||
distance(const cv::Vec3f& p_j) const |
||||
{ |
||||
return std::abs(float(p_j.dot(n_) + d_)); |
||||
} |
||||
}; |
||||
|
||||
/** Planar child with a quadratic error model
|
||||
*/ |
||||
class PlaneABC: public PlaneBase |
||||
{ |
||||
public: |
||||
PlaneABC(const cv::Vec3f & m, const cv::Vec3f &n_in, int index, float sensor_error_a, float sensor_error_b, |
||||
float sensor_error_c) |
||||
: |
||||
PlaneBase(m, n_in, index), |
||||
sensor_error_a_(sensor_error_a), |
||||
sensor_error_b_(sensor_error_b), |
||||
sensor_error_c_(sensor_error_c) |
||||
{ |
||||
} |
||||
|
||||
/** The distance is now computed by taking the sensor error into account */ |
||||
inline |
||||
float |
||||
distance(const cv::Vec3f& p_j) const |
||||
{ |
||||
float cst = p_j.dot(n_) + d_; |
||||
float err = sensor_error_a_ * p_j[2] * p_j[2] + sensor_error_b_ * p_j[2] + sensor_error_c_; |
||||
if (((cst - n_[2] * err <= 0) && (cst + n_[2] * err >= 0)) || ((cst + n_[2] * err <= 0) && (cst - n_[2] * err >= 0))) |
||||
return 0; |
||||
return std::min(std::abs(cst - err), std::abs(cst + err)); |
||||
} |
||||
private: |
||||
float sensor_error_a_; |
||||
float sensor_error_b_; |
||||
float sensor_error_c_; |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** The PlaneGrid contains statistic about the individual tiles
|
||||
*/ |
||||
class PlaneGrid |
||||
{ |
||||
public: |
||||
PlaneGrid(const cv::Mat_<cv::Vec3f> & points3d, int block_size) |
||||
: |
||||
block_size_(block_size) |
||||
{ |
||||
// Figure out some dimensions
|
||||
int mini_rows = points3d.rows / block_size; |
||||
if (points3d.rows % block_size != 0) |
||||
++mini_rows; |
||||
|
||||
int mini_cols = points3d.cols / block_size; |
||||
if (points3d.cols % block_size != 0) |
||||
++mini_cols; |
||||
|
||||
// Compute all the interesting quantities
|
||||
m_.create(mini_rows, mini_cols); |
||||
n_.create(mini_rows, mini_cols); |
||||
Q_.create(points3d.rows, points3d.cols); |
||||
mse_.create(mini_rows, mini_cols); |
||||
for (int y = 0; y < mini_rows; ++y) |
||||
for (int x = 0; x < mini_cols; ++x) |
||||
{ |
||||
// Update the tiles
|
||||
cv::Matx33f Q = cv::Matx33f::zeros(); |
||||
cv::Vec3f m = cv::Vec3f(0, 0, 0); |
||||
int K = 0; |
||||
for (int j = y * block_size; j < std::min((y + 1) * block_size, points3d.rows); ++j) |
||||
{ |
||||
const cv::Vec3f * vec = points3d.ptr < cv::Vec3f > (j, x * block_size), *vec_end; |
||||
float * pointpointt = reinterpret_cast<float*>(Q_.ptr < cv::Vec<float, 9> > (j, x * block_size)); |
||||
if (x == mini_cols - 1) |
||||
vec_end = points3d.ptr < cv::Vec3f > (j, points3d.cols - 1) + 1; |
||||
else |
||||
vec_end = vec + block_size; |
||||
for (; vec != vec_end; ++vec, pointpointt += 9) |
||||
{ |
||||
if (cvIsNaN(vec->val[0])) |
||||
continue; |
||||
// Fill point*point.t()
|
||||
*pointpointt = vec->val[0] * vec->val[0]; |
||||
*(pointpointt + 1) = vec->val[0] * vec->val[1]; |
||||
*(pointpointt + 2) = vec->val[0] * vec->val[2]; |
||||
*(pointpointt + 3) = *(pointpointt + 1); |
||||
*(pointpointt + 4) = vec->val[1] * vec->val[1]; |
||||
*(pointpointt + 5) = vec->val[1] * vec->val[2]; |
||||
*(pointpointt + 6) = *(pointpointt + 2); |
||||
*(pointpointt + 7) = *(pointpointt + 5); |
||||
*(pointpointt + 8) = vec->val[2] * vec->val[2]; |
||||
|
||||
Q += *reinterpret_cast<cv::Matx33f*>(pointpointt); |
||||
m += (*vec); |
||||
++K; |
||||
} |
||||
} |
||||
if (K == 0) |
||||
{ |
||||
mse_(y, x) = std::numeric_limits<float>::max(); |
||||
continue; |
||||
} |
||||
|
||||
m /= K; |
||||
m_(y, x) = m; |
||||
|
||||
// Compute C
|
||||
cv::Matx33f C = Q - K * m * m.t(); |
||||
|
||||
// Compute n
|
||||
cv::SVD svd(C); |
||||
n_(y, x) = cv::Vec3f(svd.vt.at<float>(2, 0), svd.vt.at<float>(2, 1), svd.vt.at<float>(2, 2)); |
||||
mse_(y, x) = svd.w.at<float>(2) / K; |
||||
} |
||||
} |
||||
|
||||
/** The size of the block */ |
||||
int block_size_; |
||||
cv::Mat_<cv::Vec3f> m_; |
||||
cv::Mat_<cv::Vec3f> n_; |
||||
cv::Mat_<cv::Vec<float, 9> > Q_; |
||||
cv::Mat_<float> mse_; |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class TileQueue |
||||
{ |
||||
public: |
||||
struct PlaneTile |
||||
{ |
||||
PlaneTile(int x, int y, float mse) |
||||
: |
||||
x_(x), |
||||
y_(y), |
||||
mse_(mse) |
||||
{ |
||||
} |
||||
|
||||
bool |
||||
operator<(const PlaneTile &tile2) const |
||||
{ |
||||
return mse_ < tile2.mse_; |
||||
} |
||||
|
||||
int x_; |
||||
int y_; |
||||
float mse_; |
||||
}; |
||||
|
||||
TileQueue(const PlaneGrid &plane_grid) |
||||
{ |
||||
done_tiles_ = cv::Mat_<unsigned char>::zeros(plane_grid.mse_.rows, plane_grid.mse_.cols); |
||||
tiles_.clear(); |
||||
for (int y = 0; y < plane_grid.mse_.rows; ++y) |
||||
for (int x = 0; x < plane_grid.mse_.cols; ++x) |
||||
if (plane_grid.mse_(y, x) != std::numeric_limits<float>::max()) |
||||
// Update the tiles
|
||||
tiles_.push_back(PlaneTile(x, y, plane_grid.mse_(y, x))); |
||||
// Sort tiles by MSE
|
||||
tiles_.sort(); |
||||
} |
||||
|
||||
bool |
||||
empty() |
||||
{ |
||||
while (!tiles_.empty()) |
||||
{ |
||||
const PlaneTile & tile = tiles_.front(); |
||||
if (done_tiles_(tile.y_, tile.x_)) |
||||
tiles_.pop_front(); |
||||
else |
||||
break; |
||||
} |
||||
return tiles_.empty(); |
||||
} |
||||
|
||||
const PlaneTile & |
||||
front() const |
||||
{ |
||||
return tiles_.front(); |
||||
} |
||||
|
||||
void |
||||
remove(int y, int x) |
||||
{ |
||||
done_tiles_(y, x) = 1; |
||||
} |
||||
private: |
||||
/** The list of tiles ordered from most planar to least */ |
||||
std::list<PlaneTile> tiles_; |
||||
/** contains 1 when the tiles has been studied, 0 otherwise */ |
||||
cv::Mat_<unsigned char> done_tiles_; |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class InlierFinder |
||||
{ |
||||
public: |
||||
InlierFinder(float err, const cv::Mat_<cv::Vec3f> & points3d, const cv::Mat_<cv::Vec3f> & normals, |
||||
unsigned char plane_index, int block_size) |
||||
: |
||||
err_(err), |
||||
points3d_(points3d), |
||||
normals_(normals), |
||||
plane_index_(plane_index), |
||||
block_size_(block_size) |
||||
{ |
||||
} |
||||
|
||||
void |
||||
Find(const PlaneGrid &plane_grid, cv::Ptr<PlaneBase> & plane, TileQueue & tile_queue, |
||||
std::set<TileQueue::PlaneTile> & neighboring_tiles, cv::Mat_<unsigned char> & overall_mask, |
||||
cv::Mat_<unsigned char> & plane_mask) |
||||
{ |
||||
// Do not use reference as we pop the from later on
|
||||
TileQueue::PlaneTile tile = *(neighboring_tiles.begin()); |
||||
|
||||
// Figure the part of the image to look at
|
||||
cv::Range range_x, range_y; |
||||
int x = tile.x_ * block_size_, y = tile.y_ * block_size_; |
||||
|
||||
if (tile.x_ == plane_mask.cols - 1) |
||||
range_x = cv::Range(x, overall_mask.cols); |
||||
else |
||||
range_x = cv::Range(x, x + block_size_); |
||||
|
||||
if (tile.y_ == plane_mask.rows - 1) |
||||
range_y = cv::Range(y, overall_mask.rows); |
||||
else |
||||
range_y = cv::Range(y, y + block_size_); |
||||
|
||||
int n_valid_points = 0; |
||||
for (int yy = range_y.start; yy != range_y.end; ++yy) |
||||
{ |
||||
uchar* data = overall_mask.ptr(yy, range_x.start), *data_end = data + range_x.size(); |
||||
const cv::Vec3f* point = points3d_.ptr < cv::Vec3f > (yy, range_x.start); |
||||
const cv::Matx33f* Q_local = reinterpret_cast<const cv::Matx33f *>(plane_grid.Q_.ptr < cv::Vec<float, 9> |
||||
> (yy, range_x.start)); |
||||
|
||||
// Depending on whether you have a normal, check it
|
||||
if (!normals_.empty()) |
||||
{ |
||||
const cv::Vec3f* normal = normals_.ptr < cv::Vec3f > (yy, range_x.start); |
||||
for (; data != data_end; ++data, ++point, ++normal, ++Q_local) |
||||
{ |
||||
// Don't do anything if the point already belongs to another plane
|
||||
if (cvIsNaN(point->val[0]) || ((*data) != 255)) |
||||
continue; |
||||
|
||||
// If the point is close enough to the plane
|
||||
if (plane->distance(*point) < err_) |
||||
{ |
||||
// make sure the normals are similar to the plane
|
||||
if (std::abs(plane->n().dot(*normal)) > 0.3) |
||||
{ |
||||
// The point now belongs to the plane
|
||||
plane->UpdateStatistics(*point, *Q_local); |
||||
*data = plane_index_; |
||||
++n_valid_points; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
for (; data != data_end; ++data, ++point, ++Q_local) |
||||
{ |
||||
// Don't do anything if the point already belongs to another plane
|
||||
if (cvIsNaN(point->val[0]) || ((*data) != 255)) |
||||
continue; |
||||
|
||||
// If the point is close enough to the plane
|
||||
if (plane->distance(*point) < err_) |
||||
{ |
||||
// The point now belongs to the plane
|
||||
plane->UpdateStatistics(*point, *Q_local); |
||||
*data = plane_index_; |
||||
++n_valid_points; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
plane->UpdateParameters(); |
||||
|
||||
// Mark the front as being done and pop it
|
||||
if (n_valid_points > (range_x.size() * range_y.size()) / 2) |
||||
tile_queue.remove(tile.y_, tile.x_); |
||||
plane_mask(tile.y_, tile.x_) = 1; |
||||
neighboring_tiles.erase(neighboring_tiles.begin()); |
||||
|
||||
// Add potential neighbors of the tile
|
||||
std::vector<std::pair<int, int> > pairs; |
||||
if (tile.x_ > 0) |
||||
for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.start, range_x.start), *val_end = val |
||||
+ range_y.size() * overall_mask.step; val != val_end; val += overall_mask.step) |
||||
if (*val == plane_index_) |
||||
{ |
||||
pairs.push_back(std::pair<int, int>(tile.x_ - 1, tile.y_)); |
||||
break; |
||||
} |
||||
if (tile.x_ < plane_mask.cols - 1) |
||||
for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.start, range_x.end - 1), *val_end = val |
||||
+ range_y.size() * overall_mask.step; val != val_end; val += overall_mask.step) |
||||
if (*val == plane_index_) |
||||
{ |
||||
pairs.push_back(std::pair<int, int>(tile.x_ + 1, tile.y_)); |
||||
break; |
||||
} |
||||
if (tile.y_ > 0) |
||||
for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.start, range_x.start), *val_end = val |
||||
+ range_x.size(); val != val_end; ++val) |
||||
if (*val == plane_index_) |
||||
{ |
||||
pairs.push_back(std::pair<int, int>(tile.x_, tile.y_ - 1)); |
||||
break; |
||||
} |
||||
if (tile.y_ < plane_mask.rows - 1) |
||||
for (unsigned char * val = overall_mask.ptr<unsigned char>(range_y.end - 1, range_x.start), *val_end = val |
||||
+ range_x.size(); val != val_end; ++val) |
||||
if (*val == plane_index_) |
||||
{ |
||||
pairs.push_back(std::pair<int, int>(tile.x_, tile.y_ + 1)); |
||||
break; |
||||
} |
||||
|
||||
for (unsigned char i = 0; i < pairs.size(); ++i) |
||||
if (!plane_mask(pairs[i].second, pairs[i].first)) |
||||
neighboring_tiles.insert( |
||||
TileQueue::PlaneTile(pairs[i].first, pairs[i].second, plane_grid.mse_(pairs[i].second, pairs[i].first))); |
||||
} |
||||
|
||||
private: |
||||
float err_; |
||||
const cv::Mat_<cv::Vec3f> & points3d_; |
||||
const cv::Mat_<cv::Vec3f> & normals_; |
||||
unsigned char plane_index_; |
||||
/** THe block size as defined in the main algorithm */ |
||||
int block_size_; |
||||
} |
||||
; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace cv |
||||
{ |
||||
void |
||||
RgbdPlane::operator()(InputArray points3d_in, OutputArray mask_out, OutputArray plane_coefficients) |
||||
{ |
||||
this->operator()(points3d_in, cv::Mat(), mask_out, plane_coefficients); |
||||
} |
||||
|
||||
void |
||||
RgbdPlane::operator()(InputArray points3d_in, InputArray normals_in, OutputArray mask_out, |
||||
OutputArray plane_coefficients_out) |
||||
{ |
||||
cv::Mat_<cv::Vec3f> points3d, normals; |
||||
if (points3d_in.depth() == CV_32F) |
||||
points3d = points3d_in.getMat(); |
||||
else |
||||
points3d_in.getMat().convertTo(points3d, CV_32F); |
||||
if (!normals_in.empty()) |
||||
{ |
||||
if (normals_in.depth() == CV_32F) |
||||
normals = normals_in.getMat(); |
||||
else |
||||
normals_in.getMat().convertTo(normals, CV_32F); |
||||
} |
||||
|
||||
// Pre-computations
|
||||
mask_out.create(points3d.size(), CV_8U); |
||||
cv::Mat mask_out_mat = mask_out.getMat(); |
||||
cv::Mat_<unsigned char> mask_out_uc = (cv::Mat_<unsigned char>&) mask_out_mat; |
||||
mask_out_uc.setTo(255); |
||||
PlaneGrid plane_grid(points3d, block_size_); |
||||
TileQueue plane_queue(plane_grid); |
||||
size_t index_plane = 0; |
||||
|
||||
std::vector<cv::Vec4f> plane_coefficients; |
||||
float mse_min = threshold_ * threshold_; |
||||
|
||||
while (!plane_queue.empty()) |
||||
{ |
||||
// Get the first tile if it's good enough
|
||||
const TileQueue::PlaneTile front_tile = plane_queue.front(); |
||||
if (front_tile.mse_ > mse_min) |
||||
break; |
||||
|
||||
InlierFinder inlier_finder(threshold_, points3d, normals, index_plane, block_size_); |
||||
|
||||
// Construct the plane for the first tile
|
||||
int x = front_tile.x_, y = front_tile.y_; |
||||
const cv::Vec3f & n = plane_grid.n_(y, x); |
||||
cv::Ptr<PlaneBase> plane; |
||||
if ((sensor_error_a_ == 0) && (sensor_error_b_ == 0) && (sensor_error_c_ == 0)) |
||||
plane = cv::Ptr<PlaneBase>(new Plane(plane_grid.m_(y, x), n, index_plane)); |
||||
else |
||||
plane = cv::Ptr<PlaneBase>(new PlaneABC(plane_grid.m_(y, x), n, index_plane, sensor_error_a_, sensor_error_b_, sensor_error_c_)); |
||||
|
||||
cv::Mat_<unsigned char> plane_mask = cv::Mat_<unsigned char>::zeros(points3d.rows / block_size_, |
||||
points3d.cols / block_size_); |
||||
std::set<TileQueue::PlaneTile> neighboring_tiles; |
||||
neighboring_tiles.insert(front_tile); |
||||
plane_queue.remove(front_tile.y_, front_tile.x_); |
||||
|
||||
// Process all the neighboring tiles
|
||||
while (!neighboring_tiles.empty()) |
||||
inlier_finder.Find(plane_grid, plane, plane_queue, neighboring_tiles, mask_out_uc, plane_mask); |
||||
|
||||
// Don't record the plane if it's empty
|
||||
if (plane->empty()) |
||||
continue; |
||||
// Don't record the plane if it's smaller than asked
|
||||
if (plane->K() < min_size_) { |
||||
// Reset the plane index in the mask
|
||||
for (y = 0; y < plane_mask.rows; ++y) |
||||
for (x = 0; x < plane_mask.cols; ++x) { |
||||
if (!plane_mask(y, x)) |
||||
continue; |
||||
// Go over the tile
|
||||
for (int yy = y * block_size_; |
||||
yy < std::min((y + 1) * block_size_, mask_out_uc.rows); ++yy) { |
||||
uchar* data = mask_out_uc.ptr(yy, x * block_size_); |
||||
uchar* data_end = data |
||||
+ std::min(block_size_, |
||||
mask_out_uc.cols - x * block_size_); |
||||
for (; data != data_end; ++data) { |
||||
if (*data == index_plane) |
||||
*data = 255; |
||||
} |
||||
} |
||||
} |
||||
continue; |
||||
} |
||||
|
||||
++index_plane; |
||||
if (index_plane >= 255) |
||||
break; |
||||
cv::Vec4f coeffs(plane->n()[0], plane->n()[1], plane->n()[2], plane->d()); |
||||
if (coeffs(2) > 0) |
||||
coeffs = -coeffs; |
||||
plane_coefficients.push_back(coeffs); |
||||
}; |
||||
|
||||
// Fill the plane coefficients
|
||||
if (plane_coefficients.empty()) |
||||
return; |
||||
plane_coefficients_out.create(plane_coefficients.size(), 1, CV_32FC4); |
||||
cv::Mat plane_coefficients_mat = plane_coefficients_out.getMat(); |
||||
float* data = plane_coefficients_mat.ptr<float>(0); |
||||
for(size_t i=0; i<plane_coefficients.size(); ++i) |
||||
for(uchar j=0; j<4; ++j, ++data) |
||||
*data = plane_coefficients[i][j]; |
||||
} |
||||
} |
||||
@ -0,0 +1,115 @@ |
||||
/*
|
||||
* Software License Agreement (BSD License) |
||||
* |
||||
* Copyright (c) 2012, Willow Garage, Inc. |
||||
* All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* * Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* * Redistributions in binary form must reproduce the above |
||||
* copyright notice, this list of conditions and the following |
||||
* disclaimer in the documentation and/or other materials provided |
||||
* with the distribution. |
||||
* * Neither the name of Willow Garage, Inc. nor the names of its |
||||
* contributors may be used to endorse or promote products derived |
||||
* from this software without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
*/ |
||||
|
||||
#include <opencv2/core.hpp> |
||||
#include <opencv2/rgbd.hpp> |
||||
#include "opencv2/core/utility.hpp" |
||||
#include "opencv2/core/private.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
CV_INIT_ALGORITHM(DepthCleaner, "RGBD.DepthCleaner", |
||||
obj.info()->addParam(obj, "window_size", obj.window_size_); |
||||
obj.info()->addParam(obj, "depth", obj.depth_); |
||||
obj.info()->addParam(obj, "method", obj.method_)) |
||||
|
||||
CV_INIT_ALGORITHM(RgbdNormals, "RGBD.RgbdNormals", |
||||
obj.info()->addParam(obj, "rows", obj.rows_); |
||||
obj.info()->addParam(obj, "cols", obj.cols_); |
||||
obj.info()->addParam(obj, "window_size", obj.window_size_); |
||||
obj.info()->addParam(obj, "depth", obj.depth_); |
||||
obj.info()->addParam(obj, "K", obj.K_); |
||||
obj.info()->addParam(obj, "method", obj.method_)) |
||||
|
||||
CV_INIT_ALGORITHM(RgbdPlane, "RGBD.RgbdPlane", |
||||
obj.info()->addParam(obj, "block_size", obj.block_size_); |
||||
obj.info()->addParam(obj, "min_size", obj.min_size_); |
||||
obj.info()->addParam(obj, "method", obj.method_); |
||||
obj.info()->addParam(obj, "threshold", obj.threshold_); |
||||
obj.info()->addParam(obj, "sensor_error_a", obj.sensor_error_a_); |
||||
obj.info()->addParam(obj, "sensor_error_b", obj.sensor_error_b_); |
||||
obj.info()->addParam(obj, "sensor_error_c", obj.sensor_error_c_)) |
||||
|
||||
CV_INIT_ALGORITHM(RgbdOdometry, "RGBD.RgbdOdometry", |
||||
obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix); |
||||
obj.info()->addParam(obj, "minDepth", obj.minDepth); |
||||
obj.info()->addParam(obj, "maxDepth", obj.maxDepth); |
||||
obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff); |
||||
obj.info()->addParam(obj, "iterCounts", obj.iterCounts); |
||||
obj.info()->addParam(obj, "minGradientMagnitudes", obj.minGradientMagnitudes); |
||||
obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart); |
||||
obj.info()->addParam(obj, "transformType", obj.transformType); |
||||
obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation); |
||||
obj.info()->addParam(obj, "maxRotation", obj.maxRotation);) |
||||
|
||||
CV_INIT_ALGORITHM(ICPOdometry, "RGBD.ICPOdometry", |
||||
obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix); |
||||
obj.info()->addParam(obj, "minDepth", obj.minDepth); |
||||
obj.info()->addParam(obj, "maxDepth", obj.maxDepth); |
||||
obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff); |
||||
obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart); |
||||
obj.info()->addParam(obj, "iterCounts", obj.iterCounts); |
||||
obj.info()->addParam(obj, "transformType", obj.transformType); |
||||
obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation); |
||||
obj.info()->addParam(obj, "maxRotation", obj.maxRotation); |
||||
obj.info()->addParam(obj, "normalsComputer", obj.normalsComputer, true);) |
||||
|
||||
CV_INIT_ALGORITHM(RgbdICPOdometry, "RGBD.RgbdICPOdometry", |
||||
obj.info()->addParam(obj, "cameraMatrix", obj.cameraMatrix); |
||||
obj.info()->addParam(obj, "minDepth", obj.minDepth); |
||||
obj.info()->addParam(obj, "maxDepth", obj.maxDepth); |
||||
obj.info()->addParam(obj, "maxDepthDiff", obj.maxDepthDiff); |
||||
obj.info()->addParam(obj, "maxPointsPart", obj.maxPointsPart); |
||||
obj.info()->addParam(obj, "iterCounts", obj.iterCounts); |
||||
obj.info()->addParam(obj, "minGradientMagnitudes", obj.minGradientMagnitudes); |
||||
obj.info()->addParam(obj, "transformType", obj.transformType); |
||||
obj.info()->addParam(obj, "maxTranslation", obj.maxTranslation); |
||||
obj.info()->addParam(obj, "maxRotation", obj.maxRotation); |
||||
obj.info()->addParam(obj, "normalsComputer", obj.normalsComputer, true);) |
||||
|
||||
bool |
||||
initModule_rgbd(void); |
||||
bool |
||||
initModule_rgbd(void) |
||||
{ |
||||
bool all = true; |
||||
all &= !RgbdNormals_info_auto.name().empty(); |
||||
all &= !RgbdPlane_info_auto.name().empty(); |
||||
all &= !RgbdOdometry_info_auto.name().empty(); |
||||
all &= !ICPOdometry_info_auto.name().empty(); |
||||
all &= !RgbdICPOdometry_info_auto.name().empty(); |
||||
return all; |
||||
} |
||||
} |
||||
@ -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 <opencv2/rgbd.hpp> |
||||
#include <limits> |
||||
|
||||
#include "utils.h" |
||||
|
||||
namespace cv |
||||
{ |
||||
/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
|
||||
* by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN() |
||||
* Otherwise, the image is simply converted to floats |
||||
* @param in_in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters |
||||
* (as done with the Microsoft Kinect), it is assumed in meters) |
||||
* @param depth the desired output depth (floats or double) |
||||
* @param out_out The rescaled float depth image |
||||
*/ |
||||
void |
||||
rescaleDepth(InputArray in_in, int depth, OutputArray out_out) |
||||
{ |
||||
cv::Mat in = in_in.getMat(); |
||||
CV_Assert(in.type() == CV_64FC1 || in.type() == CV_32FC1 || in.type() == CV_16UC1 || in.type() == CV_16SC1); |
||||
CV_Assert(depth == CV_64FC1 || depth == CV_32FC1); |
||||
|
||||
int in_depth = in.depth(); |
||||
|
||||
out_out.create(in.size(), depth); |
||||
cv::Mat out = out_out.getMat(); |
||||
if (in_depth == CV_16U) |
||||
{ |
||||
in.convertTo(out, depth, 1 / 1000.0); //convert to float so that it is in meters
|
||||
cv::Mat valid_mask = in == std::numeric_limits<uint16_t>::min(); // Should we do std::numeric_limits<uint16_t>::max() too ?
|
||||
out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
|
||||
} |
||||
if (in_depth == CV_16S) |
||||
{ |
||||
in.convertTo(out, depth, 1 / 1000.0); //convert to float so tha$
|
||||
cv::Mat valid_mask = (in == std::numeric_limits<int16_t>::min()) | (in == std::numeric_limits<int16_t>::max()); // Should we do std::numeric_limits<uint16_t>::max() too ?
|
||||
out.setTo(std::numeric_limits<float>::quiet_NaN(), valid_mask); //set a$
|
||||
} |
||||
if ((in_depth == CV_32F) || (in_depth == CV_64F)) |
||||
in.convertTo(out, depth); |
||||
} |
||||
} |
||||
@ -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 <opencv2/rgbd.hpp> |
||||
|
||||
/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
|
||||
* by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN() |
||||
* Otherwise, the image is simply converted to floats |
||||
* @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters |
||||
* (as done with the Microsoft Kinect), it is assumed in meters) |
||||
* @param the desired output depth (floats or double) |
||||
* @param out The rescaled float depth image |
||||
*/ |
||||
template<typename T> |
||||
void |
||||
rescaleDepthTemplated(const cv::Mat& in, cv::Mat& out); |
||||
|
||||
template<> |
||||
inline void |
||||
rescaleDepthTemplated<float>(const cv::Mat& in, cv::Mat& out) |
||||
{ |
||||
rescaleDepth(in, CV_32F, out); |
||||
} |
||||
|
||||
template<> |
||||
inline void |
||||
rescaleDepthTemplated<double>(const cv::Mat& in, cv::Mat& out) |
||||
{ |
||||
rescaleDepth(in, CV_64F, out); |
||||
} |
||||
|
||||
#endif /* __cplusplus */ |
||||
|
||||
#endif |
||||
|
||||
/* End of file. */ |
||||
@ -0,0 +1,3 @@ |
||||
#include "test_precomp.hpp" |
||||
|
||||
CV_TEST_MAIN("rgbd") |
||||
@ -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 <stdexcept> |
||||
|
||||
#include <opencv2/contrib.hpp> |
||||
#include <opencv2/rgbd.hpp> |
||||
|
||||
#include "test_precomp.hpp" |
||||
|
||||
cv::Point3f |
||||
rayPlaneIntersection(cv::Point2f uv, const cv::Mat& centroid, const cv::Mat& normal, const cv::Mat_<float>& Kinv); |
||||
|
||||
cv::Vec3f |
||||
rayPlaneIntersection(const cv::Vec3d& uv1, double centroid_dot_normal, const cv::Vec3d& normal, |
||||
const cv::Matx33d& Kinv); |
||||
cv::Vec3f |
||||
rayPlaneIntersection(const cv::Vec3d& uv1, double centroid_dot_normal, const cv::Vec3d& normal, const cv::Matx33d& Kinv) |
||||
{ |
||||
|
||||
cv::Matx31d L = Kinv * uv1; //a ray passing through camera optical center
|
||||
//and uv.
|
||||
L = L * (1.0 / cv::norm(L)); |
||||
double LdotNormal = L.dot(normal); |
||||
double d; |
||||
if (std::fabs(LdotNormal) > 1e-9) |
||||
{ |
||||
d = centroid_dot_normal / LdotNormal; |
||||
} |
||||
else |
||||
{ |
||||
d = 1.0; |
||||
std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl; |
||||
std::cout << "contents of L, Normal: " << cv::Mat(L) << ", " << cv::Mat(normal) << std::endl; |
||||
} |
||||
cv::Vec3f xyz(d * L(0), d * L(1), d * L(2)); |
||||
return xyz; |
||||
} |
||||
|
||||
cv::Point3f |
||||
rayPlaneIntersection(cv::Point2f uv, const cv::Mat& centroid, const cv::Mat& normal, const cv::Mat_<float>& Kinv) |
||||
{ |
||||
cv::Matx33d dKinv(Kinv); |
||||
cv::Vec3d dNormal(normal); |
||||
return rayPlaneIntersection(cv::Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv); |
||||
} |
||||
|
||||
const int W = 640; |
||||
const int H = 480; |
||||
int window_size = 5; |
||||
float focal_length = 525; |
||||
float cx = W / 2.f + 0.5f; |
||||
float cy = H / 2.f + 0.5f; |
||||
|
||||
cv::Mat K = (cv::Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1); |
||||
cv::Mat Kinv = K.inv(); |
||||
|
||||
static cv::RNG rng; |
||||
struct Plane |
||||
{ |
||||
|
||||
cv::Vec3d n, p; |
||||
double p_dot_n; |
||||
Plane() |
||||
{ |
||||
n[0] = rng.uniform(-0.5, 0.5); |
||||
n[1] = rng.uniform(-0.5, 0.5); |
||||
n[2] = -0.3; //rng.uniform(-1.f, 0.5f);
|
||||
n = n / cv::norm(n); |
||||
set_d(rng.uniform(-2.0, 0.6)); |
||||
} |
||||
|
||||
void |
||||
set_d(float d) |
||||
{ |
||||
p = cv::Vec3d(0, 0, d / n[2]); |
||||
p_dot_n = p.dot(n); |
||||
} |
||||
|
||||
cv::Vec3f |
||||
intersection(float u, float v, const cv::Matx33f& Kinv_in) const |
||||
{ |
||||
return rayPlaneIntersection(cv::Vec3d(u, v, 1), p_dot_n, n, Kinv_in); |
||||
} |
||||
}; |
||||
|
||||
void |
||||
gen_points_3d(std::vector<Plane>& planes_out, cv::Mat_<unsigned char> &plane_mask, cv::Mat& points3d, cv::Mat& normals, |
||||
int n_planes); |
||||
void |
||||
gen_points_3d(std::vector<Plane>& planes_out, cv::Mat_<unsigned char> &plane_mask, cv::Mat& points3d, cv::Mat& normals, |
||||
int n_planes) |
||||
{ |
||||
std::vector<Plane> planes; |
||||
for (int i = 0; i < n_planes; i++) |
||||
{ |
||||
Plane px; |
||||
for (int j = 0; j < 1; j++) |
||||
{ |
||||
px.set_d(rng.uniform(-3.f, -0.5f)); |
||||
planes.push_back(px); |
||||
} |
||||
} |
||||
cv::Mat_ < cv::Vec3f > outp(H, W); |
||||
cv::Mat_ < cv::Vec3f > outn(H, W); |
||||
plane_mask.create(H, W); |
||||
|
||||
// n ( r - r_0) = 0
|
||||
// n * r_0 = d
|
||||
//
|
||||
// r_0 = (0,0,0)
|
||||
// r[0]
|
||||
for (int v = 0; v < H; v++) |
||||
{ |
||||
for (int u = 0; u < W; u++) |
||||
{ |
||||
unsigned int plane_index = (u / float(W)) * planes.size(); |
||||
Plane plane = planes[plane_index]; |
||||
outp(v, u) = plane.intersection(u, v, Kinv); |
||||
outn(v, u) = plane.n; |
||||
plane_mask(v, u) = plane_index; |
||||
} |
||||
} |
||||
planes_out = planes; |
||||
points3d = outp; |
||||
normals = outn; |
||||
} |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class CV_RgbdNormalsTest: public cvtest::BaseTest |
||||
{ |
||||
public: |
||||
CV_RgbdNormalsTest() |
||||
{ |
||||
} |
||||
~CV_RgbdNormalsTest() |
||||
{ |
||||
} |
||||
protected: |
||||
void |
||||
run(int) |
||||
{ |
||||
try |
||||
{ |
||||
cv::Mat_<unsigned char> plane_mask; |
||||
for (unsigned char i = 0; i < 3; ++i) |
||||
{ |
||||
cv::RgbdNormals::RGBD_NORMALS_METHOD method; |
||||
// inner vector: whether it's 1 plane or 3 planes
|
||||
// outer vector: float or double
|
||||
std::vector<std::vector<float> > errors(2); |
||||
errors[0].resize(2); |
||||
errors[1].resize(2); |
||||
switch (i) |
||||
{ |
||||
case 0: |
||||
method = cv::RgbdNormals::RGBD_NORMALS_METHOD_FALS; |
||||
std::cout << std::endl << "*** FALS" << std::endl; |
||||
errors[0][0] = 0.006; |
||||
errors[0][1] = 0.03; |
||||
errors[1][0] = 0.00008; |
||||
errors[1][1] = 0.02; |
||||
break; |
||||
case 1: |
||||
method = cv::RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD; |
||||
std::cout << std::endl << "*** LINEMOD" << std::endl; |
||||
errors[0][0] = 0.04; |
||||
errors[0][1] = 0.07; |
||||
errors[1][0] = 0.05; |
||||
errors[1][1] = 0.08; |
||||
break; |
||||
case 2: |
||||
method = cv::RgbdNormals::RGBD_NORMALS_METHOD_SRI; |
||||
std::cout << std::endl << "*** SRI" << std::endl; |
||||
errors[0][0] = 0.02; |
||||
errors[0][1] = 0.04; |
||||
errors[1][0] = 0.02; |
||||
errors[1][1] = 0.04; |
||||
break; |
||||
} |
||||
|
||||
for (unsigned char j = 0; j < 2; ++j) |
||||
{ |
||||
int depth = (j % 2 == 0) ? CV_32F : CV_64F; |
||||
if (depth == CV_32F) |
||||
std::cout << "* float" << std::endl; |
||||
else |
||||
std::cout << "* double" << std::endl; |
||||
|
||||
cv::RgbdNormals normals_computer(H, W, depth, K, 5, method); |
||||
normals_computer.initialize(); |
||||
|
||||
std::vector<Plane> plane_params; |
||||
cv::Mat points3d, ground_normals; |
||||
// 1 plane, continuous scene, very low error..
|
||||
std::cout << "1 plane" << std::endl; |
||||
float err_mean = 0; |
||||
for (int ii = 0; ii < 5; ++ii) |
||||
{ |
||||
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1); |
||||
err_mean += testit(points3d, ground_normals, normals_computer); |
||||
} |
||||
std::cout << "mean diff: " << (err_mean / 5) << std::endl; |
||||
EXPECT_LE(err_mean/5, errors[j][0])<< " thresh: " << errors[j][0] << std::endl; |
||||
|
||||
// 3 discontinuities, more error expected.
|
||||
std::cout << "3 planes" << std::endl; |
||||
err_mean = 0; |
||||
for (int ii = 0; ii < 5; ++ii) |
||||
{ |
||||
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3); |
||||
err_mean += testit(points3d, ground_normals, normals_computer); |
||||
} |
||||
std::cout << "mean diff: " << (err_mean / 5) << std::endl; |
||||
EXPECT_LE(err_mean/5, errors[j][1])<< "mean diff: " << (err_mean/5) << " thresh: " << errors[j][1] << std::endl; |
||||
} |
||||
} |
||||
|
||||
//TODO test NaNs in data
|
||||
|
||||
} catch (...) |
||||
{ |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); |
||||
} |
||||
ts->set_failed_test_info(cvtest::TS::OK); |
||||
} |
||||
|
||||
float |
||||
testit(const cv::Mat & points3d, const cv::Mat & in_ground_normals, const cv::RgbdNormals & normals_computer) |
||||
{ |
||||
cv::TickMeter tm; |
||||
tm.start(); |
||||
cv::Mat in_normals; |
||||
if (normals_computer.method() == cv::RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD) |
||||
{ |
||||
std::vector<cv::Mat> channels; |
||||
cv::split(points3d, channels); |
||||
normals_computer(channels[2], in_normals); |
||||
} |
||||
else |
||||
normals_computer(points3d, in_normals); |
||||
tm.stop(); |
||||
|
||||
cv::Mat_<cv::Vec3f> normals, ground_normals; |
||||
in_normals.convertTo(normals, CV_32FC3); |
||||
in_ground_normals.convertTo(ground_normals, CV_32FC3); |
||||
|
||||
float err = 0; |
||||
for (int y = 0; y < normals.rows; ++y) |
||||
for (int x = 0; x < normals.cols; ++x) |
||||
{ |
||||
cv::Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x); |
||||
vec1 = vec1 / cv::norm(vec1); |
||||
vec2 = vec2 / cv::norm(vec2); |
||||
|
||||
float dot = vec1.dot(vec2); |
||||
// Just for rounding errors
|
||||
if (std::abs(dot) < 1) |
||||
err += std::min(std::acos(dot), std::acos(-dot)); |
||||
} |
||||
|
||||
err /= normals.rows * normals.cols; |
||||
std::cout << "Average error: " << err << " Speed: " << tm.getTimeMilli() << " ms" << std::endl; |
||||
return err; |
||||
} |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class CV_RgbdPlaneTest: public cvtest::BaseTest |
||||
{ |
||||
public: |
||||
CV_RgbdPlaneTest() |
||||
{ |
||||
} |
||||
~CV_RgbdPlaneTest() |
||||
{ |
||||
} |
||||
protected: |
||||
void |
||||
run(int) |
||||
{ |
||||
try |
||||
{ |
||||
cv::RgbdPlane plane_computer; |
||||
|
||||
std::vector<Plane> planes; |
||||
cv::Mat points3d, ground_normals; |
||||
cv::Mat_<unsigned char> plane_mask; |
||||
gen_points_3d(planes, plane_mask, points3d, ground_normals, 1); |
||||
testit(planes, plane_mask, points3d, plane_computer); // 1 plane, continuous scene, very low error..
|
||||
for (int ii = 0; ii < 10; ii++) |
||||
{ |
||||
gen_points_3d(planes, plane_mask, points3d, ground_normals, 3); //three planes
|
||||
testit(planes, plane_mask, points3d, plane_computer); // 3 discontinuities, more error expected.
|
||||
} |
||||
} catch (...) |
||||
{ |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); |
||||
} |
||||
ts->set_failed_test_info(cvtest::TS::OK); |
||||
} |
||||
|
||||
void |
||||
testit(const std::vector<Plane> & gt_planes, const cv::Mat & gt_plane_mask, const cv::Mat & points3d, |
||||
cv::RgbdPlane & plane_computer) |
||||
{ |
||||
for (char i_test = 0; i_test < 2; ++i_test) |
||||
{ |
||||
cv::TickMeter tm1, tm2; |
||||
cv::Mat plane_mask; |
||||
std::vector<cv::Vec4f> plane_coefficients; |
||||
|
||||
if (i_test == 0) |
||||
{ |
||||
tm1.start(); |
||||
// First, get the normals
|
||||
int depth = CV_32F; |
||||
cv::RgbdNormals normals_computer(H, W, depth, K, 5, cv::RgbdNormals::RGBD_NORMALS_METHOD_FALS); |
||||
cv::Mat normals; |
||||
normals_computer(points3d, normals); |
||||
tm1.stop(); |
||||
|
||||
tm2.start(); |
||||
plane_computer(points3d, normals, plane_mask, plane_coefficients); |
||||
tm2.stop(); |
||||
} |
||||
else |
||||
{ |
||||
tm2.start(); |
||||
plane_computer(points3d, plane_mask, plane_coefficients); |
||||
tm2.stop(); |
||||
} |
||||
|
||||
// Compare each found plane to each ground truth plane
|
||||
size_t n_planes = plane_coefficients.size(); |
||||
size_t n_gt_planes = gt_planes.size(); |
||||
cv::Mat_<int> matching(n_gt_planes, n_planes); |
||||
for (size_t j = 0; j < n_gt_planes; ++j) |
||||
{ |
||||
cv::Mat gt_mask = gt_plane_mask == j; |
||||
int n_gt = cv::countNonZero(gt_mask); |
||||
int n_max = 0, i_max = 0; |
||||
for (size_t i = 0; i < n_planes; ++i) |
||||
{ |
||||
cv::Mat dst; |
||||
cv::bitwise_and(gt_mask, plane_mask == i, dst); |
||||
matching(j, i) = cv::countNonZero(dst); |
||||
if (matching(j, i) > n_max) |
||||
{ |
||||
n_max = matching(j, i); |
||||
i_max = i; |
||||
} |
||||
} |
||||
// Get the best match
|
||||
ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001); |
||||
// Compare the normals
|
||||
cv::Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]); |
||||
ASSERT_GE(std::abs(gt_planes[j].n.dot(normal)), 0.95); |
||||
} |
||||
|
||||
std::cout << " Speed: "; |
||||
if (i_test == 0) |
||||
std::cout << "normals " << tm1.getTimeMilli() << " ms and "; |
||||
std::cout << "plane " << tm2.getTimeMilli() << " ms " << std::endl; |
||||
} |
||||
} |
||||
}; |
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
TEST(Rgbd_Normals, compute) |
||||
{ |
||||
CV_RgbdNormalsTest test; |
||||
test.safe_run(); |
||||
} |
||||
|
||||
TEST(Rgbd_Plane, compute) |
||||
{ |
||||
CV_RgbdPlaneTest test; |
||||
test.safe_run(); |
||||
} |
||||
@ -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 <opencv2/calib3d.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/imgproc.hpp> |
||||
|
||||
#include "test_precomp.hpp" |
||||
|
||||
using namespace cv; |
||||
|
||||
#define SHOW_DEBUG_LOG 0 |
||||
#define SHOW_DEBUG_IMAGES 0 |
||||
|
||||
static
|
||||
void warpFrame(const Mat& image, const Mat& depth, const Mat& rvec, const Mat& tvec, const Mat& K, |
||||
Mat& warpedImage, Mat& warpedDepth) |
||||
{ |
||||
CV_Assert(!image.empty()); |
||||
CV_Assert(image.type() == CV_8UC1); |
||||
|
||||
CV_Assert(depth.size() == image.size()); |
||||
CV_Assert(depth.type() == CV_32FC1); |
||||
|
||||
CV_Assert(!rvec.empty()); |
||||
CV_Assert(rvec.total() == 3); |
||||
CV_Assert(rvec.type() == CV_64FC1); |
||||
|
||||
CV_Assert(!tvec.empty()); |
||||
CV_Assert(tvec.size() == Size(1, 3)); |
||||
CV_Assert(tvec.type() == CV_64FC1); |
||||
|
||||
warpedImage.create(image.size(), CV_8UC1); |
||||
warpedImage = Scalar(0); |
||||
warpedDepth.create(image.size(), CV_32FC1); |
||||
warpedDepth = Scalar(FLT_MAX); |
||||
|
||||
Mat cloud; |
||||
depthTo3d(depth, K, cloud); |
||||
Mat Rt = Mat::eye(4, 4, CV_64FC1); |
||||
{ |
||||
Mat R, dst; |
||||
Rodrigues(rvec, R); |
||||
|
||||
dst = Rt(Rect(0,0,3,3)); |
||||
R.copyTo(dst); |
||||
|
||||
dst = Rt(Rect(3,0,1,3)); |
||||
tvec.copyTo(dst); |
||||
} |
||||
Mat warpedCloud, warpedImagePoints; |
||||
perspectiveTransform(cloud, warpedCloud, Rt); |
||||
projectPoints(warpedCloud.reshape(3, 1), Mat(3,1,CV_32FC1, Scalar(0)), Mat(3,1,CV_32FC1, Scalar(0)), K, Mat(1,5,CV_32FC1, Scalar(0)), warpedImagePoints); |
||||
warpedImagePoints = warpedImagePoints.reshape(2, cloud.rows); |
||||
Rect r(0, 0, image.cols, image.rows); |
||||
for(int y = 0; y < cloud.rows; y++) |
||||
{ |
||||
for(int x = 0; x < cloud.cols; x++) |
||||
{ |
||||
Point p = warpedImagePoints.at<Point2f>(y,x); |
||||
if(r.contains(p)) |
||||
{ |
||||
float curDepth = warpedDepth.at<float>(p.y, p.x); |
||||
float newDepth = warpedCloud.at<Point3f>(y, x).z; |
||||
if(newDepth < curDepth && newDepth > 0) |
||||
{ |
||||
warpedImage.at<uchar>(p.y, p.x) = image.at<uchar>(y,x); |
||||
warpedDepth.at<float>(p.y, p.x) = newDepth; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
warpedDepth.setTo(std::numeric_limits<float>::quiet_NaN(), warpedDepth > 100); |
||||
} |
||||
|
||||
static |
||||
void dilateFrame(Mat& image, Mat& depth) |
||||
{ |
||||
CV_Assert(!image.empty()); |
||||
CV_Assert(image.type() == CV_8UC1); |
||||
|
||||
CV_Assert(!depth.empty()); |
||||
CV_Assert(depth.type() == CV_32FC1); |
||||
CV_Assert(depth.size() == image.size()); |
||||
|
||||
Mat mask(image.size(), CV_8UC1, Scalar(255)); |
||||
for(int y = 0; y < depth.rows; y++) |
||||
for(int x = 0; x < depth.cols; x++) |
||||
if(cvIsNaN(depth.at<float>(y,x)) || depth.at<float>(y,x) > 10 || depth.at<float>(y,x) <= FLT_EPSILON) |
||||
mask.at<uchar>(y,x) = 0; |
||||
|
||||
image.setTo(255, ~mask); |
||||
Mat minImage; |
||||
erode(image, minImage, Mat()); |
||||
|
||||
image.setTo(0, ~mask); |
||||
Mat maxImage; |
||||
dilate(image, maxImage, Mat()); |
||||
|
||||
depth.setTo(FLT_MAX, ~mask); |
||||
Mat minDepth; |
||||
erode(depth, minDepth, Mat()); |
||||
|
||||
depth.setTo(0, ~mask); |
||||
Mat maxDepth; |
||||
dilate(depth, maxDepth, Mat()); |
||||
|
||||
Mat dilatedMask; |
||||
dilate(mask, dilatedMask, Mat(), Point(-1,-1), 1); |
||||
for(int y = 0; y < depth.rows; y++) |
||||
for(int x = 0; x < depth.cols; x++) |
||||
if(!mask.at<uchar>(y,x) && dilatedMask.at<uchar>(y,x)) |
||||
{ |
||||
image.at<uchar>(y,x) = static_cast<uchar>(0.5f * (static_cast<float>(minImage.at<uchar>(y,x)) + |
||||
static_cast<float>(maxImage.at<uchar>(y,x)))); |
||||
depth.at<float>(y,x) = 0.5f * (minDepth.at<float>(y,x) + maxDepth.at<float>(y,x)); |
||||
} |
||||
} |
||||
|
||||
class CV_OdometryTest : public cvtest::BaseTest |
||||
{ |
||||
public: |
||||
CV_OdometryTest(const Ptr<Odometry>& _odometry,
|
||||
double _maxError1,
|
||||
double _maxError5) : |
||||
odometry(_odometry),
|
||||
maxError1(_maxError1),
|
||||
maxError5(_maxError5) {} |
||||
|
||||
protected: |
||||
bool readData(Mat& image, Mat& depth) const; |
||||
static void generateRandomTransformation(Mat& R, Mat& t); |
||||
|
||||
virtual void run(int); |
||||
|
||||
Ptr<Odometry> odometry; |
||||
double maxError1; |
||||
double maxError5; |
||||
}; |
||||
|
||||
bool CV_OdometryTest::readData(Mat& image, Mat& depth) const |
||||
{ |
||||
std::string imageFilename = std::string(ts->get_data_path()) + "/odometry/rgb.png"; |
||||
std::string depthFilename = std::string(ts->get_data_path()) + "/odometry/depth.png"; |
||||
|
||||
image = imread(imageFilename, 0); |
||||
depth = imread(depthFilename, -1); |
||||
|
||||
if(image.empty()) |
||||
{ |
||||
ts->printf( cvtest::TS::LOG, "Image %s can not be read.\n", imageFilename.c_str() ); |
||||
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); |
||||
return false; |
||||
} |
||||
if(depth.empty()) |
||||
{ |
||||
ts->printf( cvtest::TS::LOG, "Depth %s can not be read.\n", depthFilename.c_str() ); |
||||
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); |
||||
ts->set_gtest_status(); |
||||
return false; |
||||
} |
||||
|
||||
CV_DbgAssert(gray.type() == CV_8UC1); |
||||
CV_DbgAssert(depth.type() == CV_16UC1); |
||||
{ |
||||
Mat depth_flt; |
||||
depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f); |
||||
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth_flt < FLT_EPSILON); |
||||
depth = depth_flt; |
||||
} |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void CV_OdometryTest::generateRandomTransformation(Mat& rvec, Mat& tvec) |
||||
{ |
||||
const float maxRotation = 3.f / 180.f * CV_PI; //rad
|
||||
const float maxTranslation = 0.02f; //m
|
||||
|
||||
RNG& rng = theRNG(); |
||||
rvec.create(3, 1, CV_64FC1); |
||||
tvec.create(3, 1, CV_64FC1); |
||||
|
||||
randu(rvec, Scalar(-1000), Scalar(1000)); |
||||
normalize(rvec, rvec, rng.uniform(0.007f, maxRotation)); |
||||
|
||||
randu(tvec, Scalar(-1000), Scalar(1000)); |
||||
normalize(tvec, tvec, rng.uniform(0.007f, maxTranslation)); |
||||
} |
||||
|
||||
void CV_OdometryTest::run(int) |
||||
{ |
||||
float fx = 525.0f, // default
|
||||
fy = 525.0f, |
||||
cx = 319.5f, |
||||
cy = 239.5f; |
||||
Mat K = Mat::eye(3,3,CV_32FC1); |
||||
{ |
||||
K.at<float>(0,0) = fx; |
||||
K.at<float>(1,1) = fy; |
||||
K.at<float>(0,2) = cx; |
||||
K.at<float>(1,2) = cy; |
||||
} |
||||
|
||||
Mat image, depth; |
||||
if(!readData(image, depth)) |
||||
return; |
||||
|
||||
odometry->set("cameraMatrix", K); |
||||
|
||||
Mat calcRt; |
||||
|
||||
// 1. Try to find Rt between the same frame (try masks also).
|
||||
bool isComputed = odometry->compute(image, depth, Mat(image.size(), CV_8UC1, Scalar(255)),
|
||||
image, depth, Mat(image.size(), CV_8UC1, Scalar(255)),
|
||||
calcRt); |
||||
if(!isComputed) |
||||
{ |
||||
ts->printf(cvtest::TS::LOG, "Can not find Rt between the same frame"); |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT); |
||||
} |
||||
double diff = norm(calcRt, Mat::eye(4,4,CV_64FC1)); |
||||
if(diff > DBL_EPSILON) |
||||
{ |
||||
ts->printf(cvtest::TS::LOG, "Incorrect transformation between the same frame (not the identity matrix), diff = %f", diff); |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); |
||||
} |
||||
|
||||
// 2. Generate random rigid body motion in some ranges several times (iterCount).
|
||||
// On each iteration an input frame is warped using generated transformation.
|
||||
// Odometry is run on the following pair: the original frame and the warped one.
|
||||
// Comparing a computed transformation with an applied one we compute 2 errors:
|
||||
// better_1time_count - count of poses which error is less than ground thrush pose,
|
||||
// better_5times_count - count of poses which error is 5 times less than ground thrush pose.
|
||||
int iterCount = 100; |
||||
int better_1time_count = 0;
|
||||
int better_5times_count = 0; |
||||
for(int iter = 0; iter < iterCount; iter++) |
||||
{ |
||||
Mat rvec, tvec; |
||||
generateRandomTransformation(rvec, tvec); |
||||
Mat warpedImage, warpedDepth; |
||||
warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth); |
||||
dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
|
||||
|
||||
isComputed = odometry->compute(image, depth, Mat(), warpedImage, warpedDepth, Mat(), calcRt); |
||||
if(!isComputed) |
||||
continue; |
||||
|
||||
Mat calcR = calcRt(Rect(0,0,3,3)), calcRvec; |
||||
Rodrigues(calcR, calcRvec); |
||||
calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows); |
||||
Mat calcTvec = calcRt(Rect(3,0,1,3)); |
||||
|
||||
#if SHOW_DEBUG_IMAGES |
||||
imshow("image", image); |
||||
imshow("warpedImage", warpedImage); |
||||
Mat resultImage, resultDepth; |
||||
warpFrame(image, depth, calcRvec, calcTvec, K, resultImage, resultDepth); |
||||
imshow("resultImage", resultImage); |
||||
waitKey(); |
||||
#endif |
||||
|
||||
|
||||
// compare rotation
|
||||
double rdiffnorm = norm(rvec - calcRvec), |
||||
rnorm = norm(rvec); |
||||
double tdiffnorm = norm(tvec - calcTvec),
|
||||
tnorm = norm(tvec); |
||||
if(rdiffnorm < rnorm && tdiffnorm < tnorm) |
||||
better_1time_count++; |
||||
if(5. * rdiffnorm < rnorm && 5 * tdiffnorm < tnorm) |
||||
better_5times_count++; |
||||
|
||||
#if SHOW_DEBUG_LOG |
||||
std::cout << "Iter " << iter << std::endl; |
||||
std::cout << "rdiffnorm " << rdiffnorm << "; rnorm " << rnorm << std::endl; |
||||
std::cout << "tdiffnorm " << tdiffnorm << "; tnorm " << tnorm << std::endl; |
||||
|
||||
std::cout << "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count << std::endl; |
||||
#endif |
||||
} |
||||
|
||||
|
||||
if(static_cast<double>(better_1time_count) < maxError1 * static_cast<double>(iterCount)) |
||||
{ |
||||
ts->printf(cvtest::TS::LOG, "\nIncorrect count of accurate poses [1st case]: %f / %f", static_cast<double>(better_1time_count), maxError1 * static_cast<double>(iterCount)); |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); |
||||
} |
||||
|
||||
if(static_cast<double>(better_5times_count) < maxError5 * static_cast<double>(iterCount)) |
||||
{ |
||||
ts->printf(cvtest::TS::LOG, "\nIncorrect count of accurate poses [2nd case]: %f / %f", static_cast<double>(better_5times_count), maxError5 * static_cast<double>(iterCount)); |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); |
||||
} |
||||
} |
||||
|
||||
/****************************************************************************************\
|
||||
* Tests registrations * |
||||
\****************************************************************************************/ |
||||
|
||||
TEST(RGBD_Odometry_Rgbd, algorithmic) |
||||
{ |
||||
CV_OdometryTest test(Algorithm::create<Odometry>("RGBD.RgbdOdometry"), 0.99, 0.94); |
||||
test.safe_run(); |
||||
} |
||||
|
||||
TEST(RGBD_Odometry_ICP, algorithmic) |
||||
{ |
||||
CV_OdometryTest test(Algorithm::create<Odometry>("RGBD.ICPOdometry"), 0.99, 0.99); |
||||
test.safe_run(); |
||||
} |
||||
|
||||
TEST(RGBD_Odometry_RgbdICP, algorithmic) |
||||
{ |
||||
CV_OdometryTest test(Algorithm::create<Odometry>("RGBD.RgbdICPOdometry"), 0.99, 0.99); |
||||
test.safe_run(); |
||||
} |
||||
@ -0,0 +1 @@ |
||||
#include "test_precomp.hpp" |
||||
@ -0,0 +1,8 @@ |
||||
#ifndef __OPENCV_TEST_PRECOMP_HPP__ |
||||
#define __OPENCV_TEST_PRECOMP_HPP__ |
||||
|
||||
#include <opencv2/ts.hpp> |
||||
#include <opencv2/rgbd.hpp> |
||||
#include <iostream> |
||||
|
||||
#endif |
||||
@ -0,0 +1,61 @@ |
||||
#include <opencv2/calib3d.hpp> |
||||
|
||||
#include "test_precomp.hpp" |
||||
|
||||
class CV_RgbdDepthTo3dTest: public cvtest::BaseTest |
||||
{ |
||||
public: |
||||
CV_RgbdDepthTo3dTest() |
||||
{ |
||||
} |
||||
~CV_RgbdDepthTo3dTest() |
||||
{ |
||||
} |
||||
protected: |
||||
void |
||||
run(int) |
||||
{ |
||||
try |
||||
{ |
||||
// K from a VGA Kinect
|
||||
cv::Mat K = (cv::Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.); |
||||
|
||||
// Create a random depth image
|
||||
cv::RNG rng; |
||||
cv::Mat_<float> depth(480, 640); |
||||
rng.fill(depth, cv::RNG::UNIFORM, 0, 100); |
||||
|
||||
// Create some 3d points on the plane
|
||||
int rows = depth.rows, cols = depth.cols; |
||||
cv::Mat_<cv::Vec3f> points3d; |
||||
cv::depthTo3d(depth, K, points3d); |
||||
|
||||
// Make sure the points belong to the plane
|
||||
cv::Mat points = points3d.reshape(1, rows*cols); |
||||
cv::Mat image_points; |
||||
cv::Mat rvec; |
||||
cv::Rodrigues(cv::Mat::eye(3,3,CV_32F),rvec); |
||||
cv::Mat tvec = (cv::Mat_<float>(1,3) << 0, 0, 0); |
||||
cv::projectPoints(points, rvec, tvec, K, cv::Mat(), image_points); |
||||
image_points = image_points.reshape(2, rows); |
||||
|
||||
float avg_diff = 0; |
||||
for (int y = 0; y < rows; ++y) |
||||
for (int x = 0; x < cols; ++x) |
||||
avg_diff += cv::norm(image_points.at<cv::Vec2f>(y,x) - cv::Vec2f(x,y)); |
||||
|
||||
// Verify the function works
|
||||
ASSERT_LE(avg_diff/rows/cols, 1e-4) << "Average error for ground truth is: " << (avg_diff / rows / cols); |
||||
} catch (...) |
||||
{ |
||||
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); |
||||
} |
||||
ts->set_failed_test_info(cvtest::TS::OK); |
||||
} |
||||
}; |
||||
|
||||
TEST(Rgbd_DepthTo3d, compute) |
||||
{ |
||||
CV_RgbdDepthTo3dTest test; |
||||
test.safe_run(); |
||||
} |
||||
|
After Width: | Height: | Size: 120 KiB |
|
After Width: | Height: | Size: 511 KiB |
Loading…
Reference in new issue