Point Cloud Library (PCL)  1.9.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 (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 (),
246  boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2));
247  float threshold = threshold_ * response_->points[indices_->front ()].intensity;
248  output.clear ();
249  output.reserve (response_->size());
250  std::vector<bool> occupency_map (response_->size (), false);
251  int width (response_->width);
252  int height (response_->height);
253  const int occupency_map_size (occupency_map.size ());
254 
255 #ifdef _OPENMP
256 #pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_)
257 #endif
258  for (int i = 0; i < occupency_map_size; ++i)
259  {
260  int idx = indices_->at (i);
261  const PointOutT& point_out = response_->points [idx];
262  if (occupency_map[idx] || point_out.intensity < threshold || !isFinite (point_out))
263  continue;
264 
265 #ifdef _OPENMP
266 #pragma omp critical
267 #endif
268  {
269  output.push_back (point_out);
270  keypoints_indices_->indices.push_back (idx);
271  }
272 
273  int u_end = std::min (width, idx % width + min_distance_);
274  int v_end = std::min (height, idx / width + min_distance_);
275  for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u)
276  for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v)
277  occupency_map[v*input_->width+u] = true;
278  }
279 
280  // if (refine_)
281  // refineCorners (output);
282 
283  output.height = 1;
284  output.width = static_cast<uint32_t> (output.size());
285  }
286 
287  // we don not change the denseness
288  output.is_dense = input_->is_dense;
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointInT, typename PointOutT, typename IntensityT> void
294 {
295  PCL_ALIGN (16) float covar [3];
296  output.clear ();
297  output.resize (input_->size ());
298  const int output_size (output.size ());
299 
300 #ifdef _OPENMP
301 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
302 #endif
303  for (int index = 0; index < output_size; ++index)
304  {
305  PointOutT& out_point = output.points [index];
306  const PointInT &in_point = (*input_).points [index];
307  out_point.intensity = 0;
308  out_point.x = in_point.x;
309  out_point.y = in_point.y;
310  out_point.z = in_point.z;
311  if (isFinite (in_point))
312  {
313  computeSecondMomentMatrix (index, covar);
314  float trace = covar [0] + covar [2];
315  if (trace != 0.f)
316  {
317  float det = covar[0] * covar[2] - covar[1] * covar[1];
318  out_point.intensity = 0.04f + det - 0.04f * trace * trace;
319  }
320  }
321  }
322 
323  output.height = input_->height;
324  output.width = input_->width;
325 }
326 
327 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
328 template <typename PointInT, typename PointOutT, typename IntensityT> void
330 {
331  PCL_ALIGN (16) float covar [3];
332  output.clear ();
333  output.resize (input_->size ());
334  const int output_size (output.size ());
335 
336 #ifdef _OPENMP
337 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
338 #endif
339  for (int index = 0; index < output_size; ++index)
340  {
341  PointOutT &out_point = output.points [index];
342  const PointInT &in_point = input_->points [index];
343  out_point.x = in_point.x;
344  out_point.y = in_point.y;
345  out_point.z = in_point.z;
346  out_point.intensity = 0;
347  if (isFinite (in_point))
348  {
349  computeSecondMomentMatrix (index, covar);
350  float trace = covar [0] + covar [2];
351  if (trace != 0)
352  {
353  float det = covar[0] * covar[2] - covar[1] * covar[1];
354  out_point.intensity = det / trace;
355  }
356  }
357  }
358 
359  output.height = input_->height;
360  output.width = input_->width;
361 }
362 
363 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
364 template <typename PointInT, typename PointOutT, typename IntensityT> void
366 {
367  PCL_ALIGN (16) float covar [3];
368  output.clear ();
369  output.resize (input_->size ());
370  const int output_size (output.size ());
371 
372 #ifdef _OPENMP
373 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
374 #endif
375  for (int index = 0; index < output_size; ++index)
376  {
377  PointOutT &out_point = output.points [index];
378  const PointInT &in_point = input_->points [index];
379  out_point.x = in_point.x;
380  out_point.y = in_point.y;
381  out_point.z = in_point.z;
382  out_point.intensity = 0;
383  if (isFinite (in_point))
384  {
385  computeSecondMomentMatrix (index, covar);
386  float trace = covar [0] + covar [2];
387  if (trace != 0)
388  {
389  float det = covar[0] * covar[2] - covar[1] * covar[1];
390  out_point.intensity = det / (trace * trace);
391  }
392  }
393  }
394 
395  output.height = input_->height;
396  output.width = input_->width;
397 }
398 
399 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
400 template <typename PointInT, typename PointOutT, typename IntensityT> void
402 {
403  PCL_ALIGN (16) float covar [3];
404  output.clear ();
405  output.resize (input_->size ());
406  const int output_size (output.size ());
407 
408 #ifdef _OPENMP
409 #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
410 #endif
411  for (int index = 0; index < output_size; ++index)
412  {
413  PointOutT &out_point = output.points [index];
414  const PointInT &in_point = input_->points [index];
415  out_point.x = in_point.x;
416  out_point.y = in_point.y;
417  out_point.z = in_point.z;
418  out_point.intensity = 0;
419  if (isFinite (in_point))
420  {
421  computeSecondMomentMatrix (index, covar);
422  // min egenvalue
423  out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f);
424  }
425  }
426 
427  output.height = input_->height;
428  output.width = input_->width;
429 }
430 
431 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432 // template <typename PointInT, typename PointOutT, typename IntensityT> void
433 // pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::refineCorners (PointCloudOut &corners) const
434 // {
435 // std::vector<int> nn_indices;
436 // std::vector<float> nn_dists;
437 
438 // Eigen::Matrix2f nnT;
439 // Eigen::Matrix2f NNT;
440 // Eigen::Matrix2f NNTInv;
441 // Eigen::Vector2f NNTp;
442 // float diff;
443 // const unsigned max_iterations = 10;
444 // #ifdef _OPENMP
445 // #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_)
446 // #endif
447 // for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
448 // {
449 // unsigned iterations = 0;
450 // do {
451 // NNT.setZero();
452 // NNTp.setZero();
453 // PointInT corner;
454 // corner.x = corners[cIdx].x;
455 // corner.y = corners[cIdx].y;
456 // corner.z = corners[cIdx].z;
457 // tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
458 // for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
459 // {
460 // if (!pcl_isfinite (normals_->points[*iIt].normal_x))
461 // continue;
462 
463 // nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
464 // NNT += nnT;
465 // NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
466 // }
467 // if (invert3x3SymMatrix (NNT, NNTInv) != 0)
468 // corners[cIdx].getVector3fMap () = NNTInv * NNTp;
469 
470 // diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
471 // } while (diff > 1e-6 && ++iterations < max_iterations);
472 // }
473 // }
474 
475 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>;
476 #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:54
void setMethod(ResponseMethod type)
set the method of the response to be calculated.
Definition: harris_2d.hpp:45
size_t size() const
Definition: point_cloud.h:448
void reserve(size_t n)
Definition: point_cloud.h:449
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
Definition: harris_2d.hpp:293
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 setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition: harris_2d.hpp:59
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
Keypoint represents the base class for key points.
Definition: keypoint.h:49
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void responseNoble(PointCloudOut &output) const
Definition: harris_2d.hpp:329
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:576
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
Definition: harris_2d.hpp:173
void setSkippedPixels(int skipped_pixels)
Set number of pixels to skip.
Definition: harris_2d.hpp:87
void setWindowWidth(int window_width)
Set window width.
Definition: harris_2d.hpp:73
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition: harris_2d.hpp:52
void responseTomasi(PointCloudOut &output) const
Definition: harris_2d.hpp:401
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
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 responseLowe(PointCloudOut &output) const
Definition: harris_2d.hpp:365