Point Cloud Library (PCL)  1.14.1-dev
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 #include <pcl/common/point_tests.h>
45 
46 namespace pcl
47 {
48 
49 template <typename PointInT, typename PointOutT, typename IntensityT> void
51 {
52  method_ = method;
53 }
54 
55 
56 template <typename PointInT, typename PointOutT, typename IntensityT> void
58 {
59  threshold_= threshold;
60 }
61 
62 
63 template <typename PointInT, typename PointOutT, typename IntensityT> void
65 {
66  refine_ = do_refine;
67 }
68 
69 
70 template <typename PointInT, typename PointOutT, typename IntensityT> void
72 {
73  nonmax_ = nonmax;
74 }
75 
76 
77 template <typename PointInT, typename PointOutT, typename IntensityT> void
79 {
80  window_width_= window_width;
81 }
82 
83 
84 template <typename PointInT, typename PointOutT, typename IntensityT> void
86 {
87  window_height_= window_height;
88 }
89 
90 
91 template <typename PointInT, typename PointOutT, typename IntensityT> void
93 {
94  skipped_pixels_= skipped_pixels;
95 }
96 
97 
98 template <typename PointInT, typename PointOutT, typename IntensityT> void
100 {
101  min_distance_ = min_distance;
102 }
103 
104 
105 template <typename PointInT, typename PointOutT, typename IntensityT> void
107 {
108  static const int width = static_cast<int> (input_->width);
109  static const int height = static_cast<int> (input_->height);
110 
111  int x = static_cast<int> (index % input_->width);
112  int y = static_cast<int> (index / input_->width);
113  // indices 0 1 2
114  // coefficients: ixix ixiy iyiy
115  std::fill_n(coefficients, 3, 0);
116 
117  int endx = std::min (width, x + half_window_width_);
118  int endy = std::min (height, y + half_window_height_);
119  for (int xx = std::max (0, x - half_window_width_); xx < endx; ++xx)
120  for (int yy = std::max (0, y - half_window_height_); yy < endy; ++yy)
121  {
122  const float& ix = derivatives_rows_ (xx,yy);
123  const float& iy = derivatives_cols_ (xx,yy);
124  coefficients[0]+= ix * ix;
125  coefficients[1]+= ix * iy;
126  coefficients[2]+= iy * iy;
127  }
128 }
129 
130 
131 template <typename PointInT, typename PointOutT, typename IntensityT> bool
133 {
135  {
136  PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ());
137  return (false);
138  }
139 
140  if (!input_->isOrganized ())
141  {
142  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
143  return (false);
144  }
145 
146  if (indices_->size () != input_->size ())
147  {
148  PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
149  return (false);
150  }
151 
152  if ((window_height_%2) == 0)
153  {
154  PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ());
155  return (false);
156  }
157 
158  if ((window_width_%2) == 0)
159  {
160  PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ());
161  return (false);
162  }
163 
164  if (window_height_ < 3 || window_width_ < 3)
165  {
166  PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
167  return (false);
168  }
169 
170  half_window_width_ = window_width_ / 2;
171  half_window_height_ = window_height_ / 2;
172 
173  return (true);
174 }
175 
176 
177 template <typename PointInT, typename PointOutT, typename IntensityT> void
179 {
180  derivatives_cols_.resize (input_->width, input_->height);
181  derivatives_rows_.resize (input_->width, input_->height);
182  //Compute cloud intensities first derivatives along columns and rows
183  //!!! nsallem 20120220 : we don't test here for density so if one term is nan the result is nan
184  int w = static_cast<int> (input_->width) - 1;
185  int h = static_cast<int> (input_->height) - 1;
186  // j = 0 --> j-1 out of range ; use 0
187  // i = 0 --> i-1 out of range ; use 0
188  derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
189  derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
190 
191  for(int i = 1; i < w; ++i)
192  {
193  derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
194  }
195 
196  derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
197  derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
198 
199  for(int j = 1; j < h; ++j)
200  {
201  // i = 0 --> i-1 out of range ; use 0
202  derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
203  for(int i = 1; i < w; ++i)
204  {
205  // derivative with respect to rows
206  derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
207 
208  // derivative with respect to cols
209  derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
210  }
211  // i = w --> w+1 out of range ; use w
212  derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
213  }
214 
215  // j = h --> j+1 out of range use h
216  derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
217  derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
218 
219  for(int i = 1; i < w; ++i)
220  {
221  derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
222  }
223  derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
224  derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
225 
226  switch (method_)
227  {
228  case HARRIS:
229  responseHarris(*response_);
230  break;
231  case NOBLE:
232  responseNoble(*response_);
233  break;
234  case LOWE:
235  responseLowe(*response_);
236  break;
237  case TOMASI:
238  responseTomasi(*response_);
239  break;
240  }
241 
242  if (!nonmax_)
243  {
244  output = *response_;
245  for (std::size_t i = 0; i < response_->size (); ++i)
246  keypoints_indices_->indices.push_back (i);
247  }
248  else
249  {
250  std::sort (indices_->begin (), indices_->end (), [this] (int p1, int p2) { return greaterIntensityAtIndices (p1, p2); });
251  const float threshold = threshold_ * (*response_)[indices_->front ()].intensity;
252  output.clear ();
253  output.reserve (response_->size());
254  std::vector<bool> occupency_map (response_->size (), false);
255  int width (response_->width);
256  int height (response_->height);
257  const int occupency_map_size (occupency_map.size ());
258 
259 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
260 #pragma omp parallel for \
261  default(none) \
262  shared(occupency_map, output) \
263  firstprivate(width, height) \
264  num_threads(threads_)
265 #else
266 #pragma omp parallel for \
267  default(none) \
268  shared(occupency_map, occupency_map_size, output, threshold) \
269  firstprivate(width, height) \
270  num_threads(threads_)
271 #endif
272  for (int i = 0; i < occupency_map_size; ++i)
273  {
274  int idx = indices_->at (i);
275  const PointOutT& point_out = response_->points [idx];
276  if (occupency_map[idx] || point_out.intensity < threshold || !isXYZFinite (point_out))
277  continue;
278 
279 #pragma omp critical
280  {
281  output.push_back (point_out);
282  keypoints_indices_->indices.push_back (idx);
283  }
284 
285  int u_end = std::min (width, idx % width + min_distance_);
286  int v_end = std::min (height, idx / width + min_distance_);
287  for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
288  for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
289  occupency_map[v*input_->width+u] = true;
290  }
291 
292  // if (refine_)
293  // refineCorners (output);
294 
295  output.height = 1;
296  output.width = output.size();
297  }
298 
299  // we don not change the denseness
300  output.is_dense = input_->is_dense;
301 }
302 
303 
304 template <typename PointInT, typename PointOutT, typename IntensityT> void
306 {
307  PCL_ALIGN (16) float covar [3];
308  output.clear ();
309  output.resize (input_->size ());
310  const int output_size (output.size ());
311 
312 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
313 #pragma omp parallel for \
314  default(none) \
315  shared(output) \
316  firstprivate(covar) \
317  num_threads(threads_)
318 #else
319 #pragma omp parallel for \
320  default(none) \
321  shared(output, output_size) \
322  firstprivate(covar) \
323  num_threads(threads_)
324 #endif
325  for (int index = 0; index < output_size; ++index)
326  {
327  PointOutT& out_point = output.points [index];
328  const PointInT &in_point = (*input_).points [index];
329  out_point.intensity = 0;
330  out_point.x = in_point.x;
331  out_point.y = in_point.y;
332  out_point.z = in_point.z;
333  if (isXYZFinite (in_point))
334  {
335  computeSecondMomentMatrix (index, covar);
336  float trace = covar [0] + covar [2];
337  if (trace != 0.f)
338  {
339  float det = covar[0] * covar[2] - covar[1] * covar[1];
340  out_point.intensity = 0.04f + det - 0.04f * trace * trace;
341  }
342  }
343  }
344 
345  output.height = input_->height;
346  output.width = input_->width;
347 }
348 
349 
350 template <typename PointInT, typename PointOutT, typename IntensityT> void
352 {
353  PCL_ALIGN (16) float covar [3];
354  output.clear ();
355  output.resize (input_->size ());
356  const int output_size (output.size ());
357 
358 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
359 #pragma omp parallel for \
360  default(none) \
361  shared(output) \
362  firstprivate(covar) \
363  num_threads(threads_)
364 #else
365 #pragma omp parallel for \
366  default(none) \
367  shared(output, output_size) \
368  firstprivate(covar) \
369  num_threads(threads_)
370 #endif
371  for (int index = 0; index < output_size; ++index)
372  {
373  PointOutT &out_point = output.points [index];
374  const PointInT &in_point = input_->points [index];
375  out_point.x = in_point.x;
376  out_point.y = in_point.y;
377  out_point.z = in_point.z;
378  out_point.intensity = 0;
379  if (isXYZFinite (in_point))
380  {
381  computeSecondMomentMatrix (index, covar);
382  float trace = covar [0] + covar [2];
383  if (trace != 0)
384  {
385  float det = covar[0] * covar[2] - covar[1] * covar[1];
386  out_point.intensity = det / trace;
387  }
388  }
389  }
390 
391  output.height = input_->height;
392  output.width = input_->width;
393 }
394 
395 
396 template <typename PointInT, typename PointOutT, typename IntensityT> void
398 {
399  PCL_ALIGN (16) float covar [3];
400  output.clear ();
401  output.resize (input_->size ());
402  const int output_size (output.size ());
403 
404 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
405 #pragma omp parallel for \
406  default(none) \
407  shared(output) \
408  firstprivate(covar) \
409  num_threads(threads_)
410 #else
411 #pragma omp parallel for \
412  default(none) \
413  shared(output, output_size) \
414  firstprivate(covar) \
415  num_threads(threads_)
416 #endif
417  for (int index = 0; index < output_size; ++index)
418  {
419  PointOutT &out_point = output.points [index];
420  const PointInT &in_point = input_->points [index];
421  out_point.x = in_point.x;
422  out_point.y = in_point.y;
423  out_point.z = in_point.z;
424  out_point.intensity = 0;
425  if (isXYZFinite (in_point))
426  {
427  computeSecondMomentMatrix (index, covar);
428  float trace = covar [0] + covar [2];
429  if (trace != 0)
430  {
431  float det = covar[0] * covar[2] - covar[1] * covar[1];
432  out_point.intensity = det / (trace * trace);
433  }
434  }
435  }
436 
437  output.height = input_->height;
438  output.width = input_->width;
439 }
440 
441 
442 template <typename PointInT, typename PointOutT, typename IntensityT> void
444 {
445  PCL_ALIGN (16) float covar [3];
446  output.clear ();
447  output.resize (input_->size ());
448  const int output_size (output.size ());
449 
450 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
451 #pragma omp parallel for \
452  default(none) \
453  shared(output) \
454  firstprivate(covar) \
455  num_threads(threads_)
456 #else
457 #pragma omp parallel for \
458  default(none) \
459  shared(output, output_size) \
460  firstprivate(covar) \
461  num_threads(threads_)
462 #endif
463  for (int index = 0; index < output_size; ++index)
464  {
465  PointOutT &out_point = output.points [index];
466  const PointInT &in_point = input_->points [index];
467  out_point.x = in_point.x;
468  out_point.y = in_point.y;
469  out_point.z = in_point.z;
470  out_point.intensity = 0;
471  if (isXYZFinite (in_point))
472  {
473  computeSecondMomentMatrix (index, covar);
474  // min egenvalue
475  out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
476  }
477  }
478 
479  output.height = input_->height;
480  output.width = input_->width;
481 }
482 
483 } // namespace pcl
484 
485 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
486 
487 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_
488 
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
Definition: harris_2d.hpp:71
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
Definition: harris_2d.hpp:92
void responseTomasi(PointCloudOut &output) const
Definition: harris_2d.hpp:443
void responseLowe(PointCloudOut &output) const
Definition: harris_2d.hpp:397
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition: harris_2d.hpp:64
void detectKeypoints(PointCloudOut &output) override
Definition: harris_2d.hpp:178
void responseNoble(PointCloudOut &output) const
Definition: harris_2d.hpp:351
void setMinimalDistance(int min_distance)
Set minimal distance between candidate keypoints.
Definition: harris_2d.hpp:99
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
Definition: harris_2d.hpp:50
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:106
void setWindowHeight(int window_height)
Set window height.
Definition: harris_2d.hpp:85
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
Definition: harris_2d.hpp:305
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: harris_2d.h:60
bool initCompute() override
Definition: harris_2d.hpp:132
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition: harris_2d.hpp:57
void setWindowWidth(int window_width)
Set window width.
Definition: harris_2d.hpp:78
Keypoint represents the base class for key points.
Definition: keypoint.h:49
constexpr bool isXYZFinite(const PointT &) noexcept
Definition: point_tests.h:125