Point Cloud Library (PCL)  1.11.0
harris_2d.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 
41 #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
42 #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_
43 
44 
45 namespace pcl
46 {
47 
48 template <typename PointInT, typename PointOutT, typename IntensityT> void
50 {
51  method_ = method;
52 }
53 
54 
55 template <typename PointInT, typename PointOutT, typename IntensityT> void
57 {
58  threshold_= threshold;
59 }
60 
61 
62 template <typename PointInT, typename PointOutT, typename IntensityT> void
64 {
65  refine_ = do_refine;
66 }
67 
68 
69 template <typename PointInT, typename PointOutT, typename IntensityT> void
71 {
72  nonmax_ = nonmax;
73 }
74 
75 
76 template <typename PointInT, typename PointOutT, typename IntensityT> void
78 {
79  window_width_= window_width;
80 }
81 
82 
83 template <typename PointInT, typename PointOutT, typename IntensityT> void
85 {
86  window_height_= window_height;
87 }
88 
89 
90 template <typename PointInT, typename PointOutT, typename IntensityT> void
92 {
93  skipped_pixels_= skipped_pixels;
94 }
95 
96 
97 template <typename PointInT, typename PointOutT, typename IntensityT> void
99 {
100  min_distance_ = min_distance;
101 }
102 
103 
104 template <typename PointInT, typename PointOutT, typename IntensityT> void
106 {
107  static const int width = static_cast<int> (input_->width);
108  static const int height = static_cast<int> (input_->height);
109 
110  int x = static_cast<int> (index % input_->width);
111  int y = static_cast<int> (index / input_->width);
112  // indices 0 1 2
113  // coefficients: ixix ixiy iyiy
114  memset (coefficients, 0, sizeof (float) * 3);
115 
116  int endx = std::min (width, x + half_window_width_);
117  int endy = std::min (height, y + half_window_height_);
118  for (int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
119  for (int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
120  {
121  const float& ix = derivatives_rows_ (xx,yy);
122  const float& iy = derivatives_cols_ (xx,yy);
123  coefficients[0]+= ix * ix;
124  coefficients[1]+= ix * iy;
125  coefficients[2]+= iy * iy;
126  }
127 }
128 
129 
130 template <typename PointInT, typename PointOutT, typename IntensityT> bool
132 {
134  {
135  PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
136  return (false);
137  }
138 
139  if (!input_->isOrganized ())
140  {
141  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
142  return (false);
143  }
144 
145  if (indices_->size () != input_->size ())
146  {
147  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
148  return (false);
149  }
150 
151  if ((window_height_%2) == 0)
152  {
153  PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
154  return (false);
155  }
156 
157  if ((window_width_%2) == 0)
158  {
159  PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
160  return (false);
161  }
162 
163  if (window_height_ < 3 || window_width_ < 3)
164  {
165  PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
166  return (false);
167  }
168 
169  half_window_width_ = window_width_ / 2;
170  half_window_height_ = window_height_ / 2;
171 
172  return (true);
173 }
174 
175 
176 template <typename PointInT, typename PointOutT, typename IntensityT> void
178 {
179  derivatives_cols_.resize (input_->width, input_->height);
180  derivatives_rows_.resize (input_->width, input_->height);
181  //Compute cloud intensities first derivatives along columns and rows
182  //!!! nsallem 20120220 : we don't test here for density so if one term is nan the result is nan
183  int w = static_cast<int> (input_->width) - 1;
184  int h = static_cast<int> (input_->height) - 1;
185  // j = 0 --> j-1 out of range ; use 0
186  // i = 0 --> i-1 out of range ; use 0
187  derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
188  derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
189 
190  for(int i = 1; i < w; ++i)
191  {
192  derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
193  }
194 
195  derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
196  derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
197 
198  for(int j = 1; j < h; ++j)
199  {
200  // i = 0 --> i-1 out of range ; use 0
201  derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
202  for(int i = 1; i < w; ++i)
203  {
204  // derivative with respect to rows
205  derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
206 
207  // derivative with respect to cols
208  derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
209  }
210  // i = w --> w+1 out of range ; use w
211  derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
212  }
213 
214  // j = h --> j+1 out of range use h
215  derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
216  derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
217 
218  for(int i = 1; i < w; ++i)
219  {
220  derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
221  }
222  derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
223  derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
224 
225  switch (method_)
226  {
227  case HARRIS:
228  responseHarris(*response_);
229  break;
230  case NOBLE:
231  responseNoble(*response_);
232  break;
233  case LOWE:
234  responseLowe(*response_);
235  break;
236  case TOMASI:
237  responseTomasi(*response_);
238  break;
239  }
240 
241  if (!nonmax_)
242  {
243  output = *response_;
244  for (std::size_t i = 0; i < response_->size (); ++i)
245  keypoints_indices_->indices.push_back (i);
246  }
247  else
248  {
249  std::sort (indices_->begin (), indices_->end (), [this] (int p1, int p2) { return greaterIntensityAtIndices (p1, p2); });
250  const float threshold = threshold_ * response_->points[indices_->front ()].intensity;
251  output.clear ();
252  output.reserve (response_->size());
253  std::vector<bool> occupency_map (response_->size (), false);
254  int width (response_->width);
255  int height (response_->height);
256  const int occupency_map_size (occupency_map.size ());
257 
258 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
259 #pragma omp parallel for \
260  default(none) \
261  shared(occupency_map, output) \
262  firstprivate(width, height) \
263  num_threads(threads_)
264 #else
265 #pragma omp parallel for \
266  default(none) \
267  shared(occupency_map, occupency_map_size, output, threshold) \
268  firstprivate(width, height) \
269  num_threads(threads_)
270 #endif
271  for (int i = 0; i < occupency_map_size; ++i)
272  {
273  int idx = indices_->at (i);
274  const PointOutT& point_out = response_->points [idx];
275  if (occupency_map[idx] || point_out.intensity < threshold || !isFinite (point_out))
276  continue;
277 
278 #pragma omp critical
279  {
280  output.push_back (point_out);
281  keypoints_indices_->indices.push_back (idx);
282  }
283 
284  int u_end = std::min (width, idx % width + min_distance_);
285  int v_end = std::min (height, idx / width + min_distance_);
286  for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
287  for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
288  occupency_map[v*input_->width+u] = true;
289  }
290 
291  // if (refine_)
292  // refineCorners (output);
293 
294  output.height = 1;
295  output.width = static_cast<std::uint32_t> (output.size());
296  }
297 
298  // we don not change the denseness
299  output.is_dense = input_->is_dense;
300 }
301 
302 
303 template <typename PointInT, typename PointOutT, typename IntensityT> void
305 {
306  PCL_ALIGN (16) float covar [3];
307  output.clear ();
308  output.resize (input_->size ());
309  const int output_size (output.size ());
310 
311 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
312 #pragma omp parallel for \
313  default(none) \
314  shared(output) \
315  private(covar) \
316  num_threads(threads_)
317 #else
318 #pragma omp parallel for \
319  default(none) \
320  shared(output, output_size) \
321  private(covar) \
322  num_threads(threads_)
323 #endif
324  for (int index = 0; index < output_size; ++index)
325  {
326  PointOutT& out_point = output.points [index];
327  const PointInT &in_point = (*input_).points [index];
328  out_point.intensity = 0;
329  out_point.x = in_point.x;
330  out_point.y = in_point.y;
331  out_point.z = in_point.z;
332  if (isFinite (in_point))
333  {
334  computeSecondMomentMatrix (index, covar);
335  float trace = covar [0] + covar [2];
336  if (trace != 0.f)
337  {
338  float det = covar[0] * covar[2] - covar[1] * covar[1];
339  out_point.intensity = 0.04f + det - 0.04f * trace * trace;
340  }
341  }
342  }
343 
344  output.height = input_->height;
345  output.width = input_->width;
346 }
347 
348 
349 template <typename PointInT, typename PointOutT, typename IntensityT> void
351 {
352  PCL_ALIGN (16) float covar [3];
353  output.clear ();
354  output.resize (input_->size ());
355  const int output_size (output.size ());
356 
357 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
358 #pragma omp parallel for \
359  default(none) \
360  shared(output) \
361  private(covar) \
362  num_threads(threads_)
363 #else
364 #pragma omp parallel for \
365  default(none) \
366  shared(output, output_size) \
367  private(covar) \
368  num_threads(threads_)
369 #endif
370  for (int index = 0; index < output_size; ++index)
371  {
372  PointOutT &out_point = output.points [index];
373  const PointInT &in_point = input_->points [index];
374  out_point.x = in_point.x;
375  out_point.y = in_point.y;
376  out_point.z = in_point.z;
377  out_point.intensity = 0;
378  if (isFinite (in_point))
379  {
380  computeSecondMomentMatrix (index, covar);
381  float trace = covar [0] + covar [2];
382  if (trace != 0)
383  {
384  float det = covar[0] * covar[2] - covar[1] * covar[1];
385  out_point.intensity = det / trace;
386  }
387  }
388  }
389 
390  output.height = input_->height;
391  output.width = input_->width;
392 }
393 
394 
395 template <typename PointInT, typename PointOutT, typename IntensityT> void
397 {
398  PCL_ALIGN (16) float covar [3];
399  output.clear ();
400  output.resize (input_->size ());
401  const int output_size (output.size ());
402 
403 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
404 #pragma omp parallel for \
405  default(none) \
406  shared(output) \
407  private(covar) \
408  num_threads(threads_)
409 #else
410 #pragma omp parallel for \
411  default(none) \
412  shared(output, output_size) \
413  private(covar) \
414  num_threads(threads_)
415 #endif
416  for (int index = 0; index < output_size; ++index)
417  {
418  PointOutT &out_point = output.points [index];
419  const PointInT &in_point = input_->points [index];
420  out_point.x = in_point.x;
421  out_point.y = in_point.y;
422  out_point.z = in_point.z;
423  out_point.intensity = 0;
424  if (isFinite (in_point))
425  {
426  computeSecondMomentMatrix (index, covar);
427  float trace = covar [0] + covar [2];
428  if (trace != 0)
429  {
430  float det = covar[0] * covar[2] - covar[1] * covar[1];
431  out_point.intensity = det / (trace * trace);
432  }
433  }
434  }
435 
436  output.height = input_->height;
437  output.width = input_->width;
438 }
439 
440 
441 template <typename PointInT, typename PointOutT, typename IntensityT> void
443 {
444  PCL_ALIGN (16) float covar [3];
445  output.clear ();
446  output.resize (input_->size ());
447  const int output_size (output.size ());
448 
449 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
450 #pragma omp parallel for \
451  default(none) \
452  shared(output) \
453  private(covar) \
454  num_threads(threads_)
455 #else
456 #pragma omp parallel for \
457  default(none) \
458  shared(output, output_size) \
459  private(covar) \
460  num_threads(threads_)
461 #endif
462  for (int index = 0; index < output_size; ++index)
463  {
464  PointOutT &out_point = output.points [index];
465  const PointInT &in_point = input_->points [index];
466  out_point.x = in_point.x;
467  out_point.y = in_point.y;
468  out_point.z = in_point.z;
469  out_point.intensity = 0;
470  if (isFinite (in_point))
471  {
472  computeSecondMomentMatrix (index, covar);
473  // min egenvalue
474  out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
475  }
476  }
477 
478  output.height = input_->height;
479  output.width = input_->width;
480 }
481 
482 } // namespace pcl
483 
484 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
485 
486 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
487 
pcl
Definition: convolution.h:46
pcl::HarrisKeypoint2D::setRefine
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition: harris_2d.hpp:63
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
pcl::HarrisKeypoint2D::setWindowWidth
void setWindowWidth(int window_width)
Set window width.
Definition: harris_2d.hpp:77
pcl::HarrisKeypoint2D::ResponseMethod
ResponseMethod
Definition: harris_2d.h:69
pcl::HarrisKeypoint2D::setNonMaxSupression
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
Definition: harris_2d.hpp:70
pcl::HarrisKeypoint2D::PointCloudOut
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: harris_2d.h:60
pcl::HarrisKeypoint2D::initCompute
bool initCompute() override
Definition: harris_2d.hpp:131
pcl::HarrisKeypoint2D::setThreshold
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition: harris_2d.hpp:56
pcl::HarrisKeypoint2D::computeSecondMomentMatrix
void computeSecondMomentMatrix(std::size_t pos, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over intensities given by the ...
Definition: harris_2d.hpp:105
pcl::HarrisKeypoint2D::responseNoble
void responseNoble(PointCloudOut &output) const
Definition: harris_2d.hpp:350
pcl::HarrisKeypoint2D::detectKeypoints
void detectKeypoints(PointCloudOut &output) override
Definition: harris_2d.hpp:177
pcl::HarrisKeypoint2D::setSkippedPixels
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
Definition: harris_2d.hpp:91
pcl::HarrisKeypoint2D::responseHarris
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
Definition: harris_2d.hpp:304
pcl::HarrisKeypoint2D::setMinimalDistance
void setMinimalDistance(int min_distance)
Set minimal distance between candidate keypoints.
Definition: harris_2d.hpp:98
pcl::HarrisKeypoint2D::setMethod
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
Definition: harris_2d.hpp:49
pcl::HarrisKeypoint2D::setWindowHeight
void setWindowHeight(int window_height)
Set window height.
Definition: harris_2d.hpp:84
pcl::HarrisKeypoint2D::responseLowe
void responseLowe(PointCloudOut &output) const
Definition: harris_2d.hpp:396
pcl::Keypoint
Keypoint represents the base class for key points.
Definition: keypoint.h:49
pcl::HarrisKeypoint2D::responseTomasi
void responseTomasi(PointCloudOut &output) const
Definition: harris_2d.hpp:442