489 lines
16 KiB
C++
489 lines
16 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2011, 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.
|
|
*
|
|
* $Id$
|
|
*
|
|
*/
|
|
|
|
|
|
#ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
|
|
#define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
|
|
|
|
#include <pcl/common/point_tests.h>
|
|
|
|
namespace pcl
|
|
{
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMethod (ResponseMethod method)
|
|
{
|
|
method_ = method;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setThreshold (float threshold)
|
|
{
|
|
threshold_= threshold;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setRefine (bool do_refine)
|
|
{
|
|
refine_ = do_refine;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setNonMaxSupression (bool nonmax)
|
|
{
|
|
nonmax_ = nonmax;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowWidth (int window_width)
|
|
{
|
|
window_width_= window_width;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowHeight (int window_height)
|
|
{
|
|
window_height_= window_height;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setSkippedPixels (int skipped_pixels)
|
|
{
|
|
skipped_pixels_= skipped_pixels;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMinimalDistance (int min_distance)
|
|
{
|
|
min_distance_ = min_distance;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::computeSecondMomentMatrix (std::size_t index, float* coefficients) const
|
|
{
|
|
static const int width = static_cast<int> (input_->width);
|
|
static const int height = static_cast<int> (input_->height);
|
|
|
|
int x = static_cast<int> (index % input_->width);
|
|
int y = static_cast<int> (index / input_->width);
|
|
// indices 0 1 2
|
|
// coefficients: ixix ixiy iyiy
|
|
memset (coefficients, 0, sizeof (float) * 3);
|
|
|
|
int endx = std::min (width, x + half_window_width_);
|
|
int endy = std::min (height, y + half_window_height_);
|
|
for (int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
|
|
for (int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
|
|
{
|
|
const float& ix = derivatives_rows_ (xx,yy);
|
|
const float& iy = derivatives_cols_ (xx,yy);
|
|
coefficients[0]+= ix * ix;
|
|
coefficients[1]+= ix * iy;
|
|
coefficients[2]+= iy * iy;
|
|
}
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> bool
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute ()
|
|
{
|
|
if (!pcl::Keypoint<PointInT, PointOutT>::initCompute ())
|
|
{
|
|
PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
|
|
return (false);
|
|
}
|
|
|
|
if (!input_->isOrganized ())
|
|
{
|
|
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
|
|
return (false);
|
|
}
|
|
|
|
if (indices_->size () != input_->size ())
|
|
{
|
|
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
|
|
return (false);
|
|
}
|
|
|
|
if ((window_height_%2) == 0)
|
|
{
|
|
PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
|
|
return (false);
|
|
}
|
|
|
|
if ((window_width_%2) == 0)
|
|
{
|
|
PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
|
|
return (false);
|
|
}
|
|
|
|
if (window_height_ < 3 || window_width_ < 3)
|
|
{
|
|
PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
|
|
return (false);
|
|
}
|
|
|
|
half_window_width_ = window_width_ / 2;
|
|
half_window_height_ = window_height_ / 2;
|
|
|
|
return (true);
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
|
|
{
|
|
derivatives_cols_.resize (input_->width, input_->height);
|
|
derivatives_rows_.resize (input_->width, input_->height);
|
|
//Compute cloud intensities first derivatives along columns and rows
|
|
//!!! nsallem 20120220 : we don't test here for density so if one term is nan the result is nan
|
|
int w = static_cast<int> (input_->width) - 1;
|
|
int h = static_cast<int> (input_->height) - 1;
|
|
// j = 0 --> j-1 out of range ; use 0
|
|
// i = 0 --> i-1 out of range ; use 0
|
|
derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
|
|
derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
|
|
|
|
for(int i = 1; i < w; ++i)
|
|
{
|
|
derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
|
|
}
|
|
|
|
derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
|
|
derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
|
|
|
|
for(int j = 1; j < h; ++j)
|
|
{
|
|
// i = 0 --> i-1 out of range ; use 0
|
|
derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
|
|
for(int i = 1; i < w; ++i)
|
|
{
|
|
// derivative with respect to rows
|
|
derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
|
|
|
|
// derivative with respect to cols
|
|
derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
|
|
}
|
|
// i = w --> w+1 out of range ; use w
|
|
derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
|
|
}
|
|
|
|
// j = h --> j+1 out of range use h
|
|
derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
|
|
derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
|
|
|
|
for(int i = 1; i < w; ++i)
|
|
{
|
|
derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
|
|
}
|
|
derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
|
|
derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
|
|
|
|
switch (method_)
|
|
{
|
|
case HARRIS:
|
|
responseHarris(*response_);
|
|
break;
|
|
case NOBLE:
|
|
responseNoble(*response_);
|
|
break;
|
|
case LOWE:
|
|
responseLowe(*response_);
|
|
break;
|
|
case TOMASI:
|
|
responseTomasi(*response_);
|
|
break;
|
|
}
|
|
|
|
if (!nonmax_)
|
|
{
|
|
output = *response_;
|
|
for (std::size_t i = 0; i < response_->size (); ++i)
|
|
keypoints_indices_->indices.push_back (i);
|
|
}
|
|
else
|
|
{
|
|
std::sort (indices_->begin (), indices_->end (), [this] (int p1, int p2) { return greaterIntensityAtIndices (p1, p2); });
|
|
const float threshold = threshold_ * (*response_)[indices_->front ()].intensity;
|
|
output.clear ();
|
|
output.reserve (response_->size());
|
|
std::vector<bool> occupency_map (response_->size (), false);
|
|
int width (response_->width);
|
|
int height (response_->height);
|
|
const int occupency_map_size (occupency_map.size ());
|
|
|
|
#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(occupency_map, output) \
|
|
firstprivate(width, height) \
|
|
num_threads(threads_)
|
|
#else
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(occupency_map, occupency_map_size, output, threshold) \
|
|
firstprivate(width, height) \
|
|
num_threads(threads_)
|
|
#endif
|
|
for (int i = 0; i < occupency_map_size; ++i)
|
|
{
|
|
int idx = indices_->at (i);
|
|
const PointOutT& point_out = response_->points [idx];
|
|
if (occupency_map[idx] || point_out.intensity < threshold || !isXYZFinite (point_out))
|
|
continue;
|
|
|
|
#pragma omp critical
|
|
{
|
|
output.push_back (point_out);
|
|
keypoints_indices_->indices.push_back (idx);
|
|
}
|
|
|
|
int u_end = std::min (width, idx % width + min_distance_);
|
|
int v_end = std::min (height, idx / width + min_distance_);
|
|
for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
|
|
for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
|
|
occupency_map[v*input_->width+u] = true;
|
|
}
|
|
|
|
// if (refine_)
|
|
// refineCorners (output);
|
|
|
|
output.height = 1;
|
|
output.width = output.size();
|
|
}
|
|
|
|
// we don not change the denseness
|
|
output.is_dense = input_->is_dense;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseHarris (PointCloudOut &output) const
|
|
{
|
|
PCL_ALIGN (16) float covar [3];
|
|
output.clear ();
|
|
output.resize (input_->size ());
|
|
const int output_size (output.size ());
|
|
|
|
#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#else
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output, output_size) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#endif
|
|
for (int index = 0; index < output_size; ++index)
|
|
{
|
|
PointOutT& out_point = output.points [index];
|
|
const PointInT &in_point = (*input_).points [index];
|
|
out_point.intensity = 0;
|
|
out_point.x = in_point.x;
|
|
out_point.y = in_point.y;
|
|
out_point.z = in_point.z;
|
|
if (isXYZFinite (in_point))
|
|
{
|
|
computeSecondMomentMatrix (index, covar);
|
|
float trace = covar [0] + covar [2];
|
|
if (trace != 0.f)
|
|
{
|
|
float det = covar[0] * covar[2] - covar[1] * covar[1];
|
|
out_point.intensity = 0.04f + det - 0.04f * trace * trace;
|
|
}
|
|
}
|
|
}
|
|
|
|
output.height = input_->height;
|
|
output.width = input_->width;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseNoble (PointCloudOut &output) const
|
|
{
|
|
PCL_ALIGN (16) float covar [3];
|
|
output.clear ();
|
|
output.resize (input_->size ());
|
|
const int output_size (output.size ());
|
|
|
|
#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#else
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output, output_size) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#endif
|
|
for (int index = 0; index < output_size; ++index)
|
|
{
|
|
PointOutT &out_point = output.points [index];
|
|
const PointInT &in_point = input_->points [index];
|
|
out_point.x = in_point.x;
|
|
out_point.y = in_point.y;
|
|
out_point.z = in_point.z;
|
|
out_point.intensity = 0;
|
|
if (isXYZFinite (in_point))
|
|
{
|
|
computeSecondMomentMatrix (index, covar);
|
|
float trace = covar [0] + covar [2];
|
|
if (trace != 0)
|
|
{
|
|
float det = covar[0] * covar[2] - covar[1] * covar[1];
|
|
out_point.intensity = det / trace;
|
|
}
|
|
}
|
|
}
|
|
|
|
output.height = input_->height;
|
|
output.width = input_->width;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseLowe (PointCloudOut &output) const
|
|
{
|
|
PCL_ALIGN (16) float covar [3];
|
|
output.clear ();
|
|
output.resize (input_->size ());
|
|
const int output_size (output.size ());
|
|
|
|
#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#else
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output, output_size) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#endif
|
|
for (int index = 0; index < output_size; ++index)
|
|
{
|
|
PointOutT &out_point = output.points [index];
|
|
const PointInT &in_point = input_->points [index];
|
|
out_point.x = in_point.x;
|
|
out_point.y = in_point.y;
|
|
out_point.z = in_point.z;
|
|
out_point.intensity = 0;
|
|
if (isXYZFinite (in_point))
|
|
{
|
|
computeSecondMomentMatrix (index, covar);
|
|
float trace = covar [0] + covar [2];
|
|
if (trace != 0)
|
|
{
|
|
float det = covar[0] * covar[2] - covar[1] * covar[1];
|
|
out_point.intensity = det / (trace * trace);
|
|
}
|
|
}
|
|
}
|
|
|
|
output.height = input_->height;
|
|
output.width = input_->width;
|
|
}
|
|
|
|
|
|
template <typename PointInT, typename PointOutT, typename IntensityT> void
|
|
HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseTomasi (PointCloudOut &output) const
|
|
{
|
|
PCL_ALIGN (16) float covar [3];
|
|
output.clear ();
|
|
output.resize (input_->size ());
|
|
const int output_size (output.size ());
|
|
|
|
#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#else
|
|
#pragma omp parallel for \
|
|
default(none) \
|
|
shared(output, output_size) \
|
|
firstprivate(covar) \
|
|
num_threads(threads_)
|
|
#endif
|
|
for (int index = 0; index < output_size; ++index)
|
|
{
|
|
PointOutT &out_point = output.points [index];
|
|
const PointInT &in_point = input_->points [index];
|
|
out_point.x = in_point.x;
|
|
out_point.y = in_point.y;
|
|
out_point.z = in_point.z;
|
|
out_point.intensity = 0;
|
|
if (isXYZFinite (in_point))
|
|
{
|
|
computeSecondMomentMatrix (index, covar);
|
|
// min egenvalue
|
|
out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
|
|
}
|
|
}
|
|
|
|
output.height = input_->height;
|
|
output.width = input_->width;
|
|
}
|
|
|
|
} // namespace pcl
|
|
|
|
#define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
|
|
|
|
#endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
|
|
|