template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
response->points.reserve (input_->points.size());
switch (method_)
{
case HARRIS:
responseHarris(*response);
break;
case NOBLE:
responseNoble(*response);
break;
case LOWE:
responseLowe(*response);
break;
case CURVATURE:
responseCurvature(*response);
break;
case TOMASI:
responseTomasi(*response);
break;
}
if (!nonmax_)
output = *response;
else
{
output.points.clear ();
output.points.reserve (response->points.size());
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads(threads_)
#endif
for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
{
if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
continue;
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
bool is_maxima = true;
for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
{
if (response->points[idx].intensity < response->points[*iIt].intensity)
{
is_maxima = false;
break;
}
}
if (is_maxima)
#ifdef _OPENMP
#pragma omp critical
#endif
output.points.push_back (response->points[idx]);
}
if (refine_)
refineCorners (output);
output.height = 1;
output.width = static_cast<uint32_t> (output.points.size());
}
// we don not change the denseness
output.is_dense = input_->is_dense;
}
template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::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 in 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;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
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;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
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;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
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;
float highest_response_;
switch (method_)
{
case HARRIS:
responseHarris(*response_, highest_response_);
break;
case NOBLE:
responseNoble(*response_, highest_response_);
break;
case LOWE:
responseLowe(*response_, highest_response_);
break;
case TOMASI:
responseTomasi(*response_, highest_response_);
break;
}
if (!nonmax_)
output = *response_;
else
{
threshold_*= highest_response_;
std::sort (indices_->begin (), indices_->end (),
boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2));
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 ());
#ifdef _OPENMP
#pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
#endif
for (int idx = 0; idx < occupency_map_size; ++idx)
{
if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx]))
continue;
#ifdef _OPENMP
#pragma omp critical
#endif
output.push_back (response_->at (indices_->at (idx)));
int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
int v_end = std::min (height, indices_->at (idx) / width + min_distance_);
for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u)
for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v)
occupency_map[v*input_->width+u] = true;
}
// if (refine_)
// refineCorners (output);
output.height = 1;
output.width = static_cast<uint32_t> (output.size());
}
// we don not change the denseness
output.is_dense = input_->is_dense;
}
