You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
320 lines
10 KiB
320 lines
10 KiB
/* |
|
* 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 "precomp.hpp" |
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
namespace cv |
|
{ |
|
namespace rgbd |
|
{ |
|
class DepthCleanerImpl |
|
{ |
|
public: |
|
DepthCleanerImpl(int window_size, int depth, 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_; |
|
DepthCleaner::DEPTH_CLEANER_METHOD method_; |
|
}; |
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
/** 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 Vec<T, 3> Vec3T; |
|
typedef Matx<T, 3, 3> Mat33T; |
|
|
|
NIL(int window_size, int depth, 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 Mat& depth_in, Mat& depth_out) const |
|
{ |
|
switch (depth_in.depth()) |
|
{ |
|
case CV_16U: |
|
{ |
|
const Mat_<unsigned short> &depth(depth_in); |
|
Mat depth_out_tmp; |
|
computeImpl<unsigned short, float>(depth, depth_out_tmp, 0.001f); |
|
depth_out_tmp.convertTo(depth_out, CV_16U); |
|
break; |
|
} |
|
case CV_32F: |
|
{ |
|
const Mat_<float> &depth(depth_in); |
|
computeImpl<float, float>(depth, depth_out, 1); |
|
break; |
|
} |
|
case CV_64F: |
|
{ |
|
const 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 Mat_<DepthDepth> &depth_in, Mat & depth_out, ContainerDepth scale) const |
|
{ |
|
const ContainerDepth theta_mean = (float)(30. * CV_PI / 180); |
|
int rows = depth_in.rows; |
|
int cols = depth_in.cols; |
|
|
|
// Precompute some data |
|
const ContainerDepth sigma_L = (float)(0.8 + 0.035 * theta_mean / (CV_PI / 2 - theta_mean)); |
|
Mat_<ContainerDepth> sigma_z(rows, cols); |
|
for (int y = 0; y < rows; ++y) |
|
for (int x = 0; x < cols; ++x) |
|
sigma_z(y, x) = (float)(0.0012 + 0.0019 * (depth_in(y, x) * scale - 0.4) * (depth_in(y, x) * scale - 0.4)); |
|
|
|
ContainerDepth difference_threshold = 10; |
|
Mat_<ContainerDepth> Dw_sum = Mat_<ContainerDepth>::zeros(rows, cols), w_sum = |
|
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 = (float)(depth_in(y, x) - depth_in(y + j, x + i)); |
|
else |
|
delta_z = (float)(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; |
|
} |
|
} |
|
} |
|
} |
|
} |
|
Mat(Dw_sum / w_sum).copyTo(depth_out); |
|
} |
|
}; |
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
/** 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 |
|
{ |
|
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_); |
|
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; |
|
} |
|
} |
|
} |
|
} |
|
}
|
|
|