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