Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
harris_keypoint3D.hpp
Go to the documentation of this file.
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  */
37 
38 #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
39 #define PCL_HARRIS_KEYPOINT_3D_IMPL_H_
40 
41 #include <pcl/keypoints/harris_keypoint3D.h>
42 #include <pcl/common/io.h>
43 #include <pcl/filters/passthrough.h>
44 #include <pcl/filters/extract_indices.h>
45 #include <pcl/features/normal_3d.h>
46 #include <pcl/features/integral_image_normal.h>
47 #include <pcl/common/time.h>
48 #include <pcl/common/centroid.h>
49 #ifdef __SSE__
50 #include <xmmintrin.h>
51 #endif
52 
54 template <typename PointInT, typename PointOutT, typename NormalT> void
55 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setMethod (ResponseMethod method)
56 {
57  method_ = method;
58 }
59 
61 template <typename PointInT, typename PointOutT, typename NormalT> void
62 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setThreshold (float threshold)
63 {
64  threshold_= threshold;
65 }
66 
68 template <typename PointInT, typename PointOutT, typename NormalT> void
69 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setRadius (float radius)
70 {
71  search_radius_ = radius;
72 }
73 
75 template <typename PointInT, typename PointOutT, typename NormalT> void
76 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setRefine (bool do_refine)
77 {
78  refine_ = do_refine;
79 }
80 
82 template <typename PointInT, typename PointOutT, typename NormalT> void
83 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNonMaxSupression (bool nonmax)
84 {
85  nonmax_ = nonmax;
86 }
87 
89 template <typename PointInT, typename PointOutT, typename NormalT> void
90 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNormals (const PointCloudNPtr &normals)
91 {
92  normals_.reset (normals.get ());
93 }
94 
96 template <typename PointInT, typename PointOutT, typename NormalT> void
97 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const std::vector<int>& neighbors, float* coefficients) const
98 {
99  unsigned count = 0;
100  // indices 0 1 2 3 4 5 6 7
101  // coefficients: xx xy xz ?? yx yy yz ??
102 #ifdef __SSE__
103  // accumulator for xx, xy, xz
104  __m128 vec1 = _mm_setzero_ps();
105  // accumulator for yy, yz, zz
106  __m128 vec2 = _mm_setzero_ps();
107 
108  __m128 norm1;
109 
110  __m128 norm2;
111 
112  float zz = 0;
113 
114  for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
115  {
116  if (pcl_isfinite (normals_->points[*iIt].normal_x))
117  {
118  // nx, ny, nz, h
119  norm1 = _mm_load_ps (&(normals_->points[*iIt].normal_x));
120 
121  // nx, nx, nx, nx
122  norm2 = _mm_set1_ps (normals_->points[*iIt].normal_x);
123 
124  // nx * nx, nx * ny, nx * nz, nx * h
125  norm2 = _mm_mul_ps (norm1, norm2);
126 
127  // accumulate
128  vec1 = _mm_add_ps (vec1, norm2);
129 
130  // ny, ny, ny, ny
131  norm2 = _mm_set1_ps (normals_->points[*iIt].normal_y);
132 
133  // ny * nx, ny * ny, ny * nz, ny * h
134  norm2 = _mm_mul_ps (norm1, norm2);
135 
136  // accumulate
137  vec2 = _mm_add_ps (vec2, norm2);
138 
139  zz += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
140  ++count;
141  }
142  }
143  if (count > 0)
144  {
145  norm2 = _mm_set1_ps (float(count));
146  vec1 = _mm_div_ps (vec1, norm2);
147  vec2 = _mm_div_ps (vec2, norm2);
148  _mm_store_ps (coefficients, vec1);
149  _mm_store_ps (coefficients + 4, vec2);
150  coefficients [7] = zz / float(count);
151  }
152  else
153  memset (coefficients, 0, sizeof (float) * 8);
154 #else
155  memset (coefficients, 0, sizeof (float) * 8);
156  for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
157  {
158  if (pcl_isfinite (normals_->points[*iIt].normal_x))
159  {
160  coefficients[0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x;
161  coefficients[1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y;
162  coefficients[2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z;
163 
164  coefficients[5] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
165  coefficients[6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
166  coefficients[7] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
167 
168  ++count;
169  }
170  }
171  if (count > 0)
172  {
173  float norm = 1.0 / float (count);
174  coefficients[0] *= norm;
175  coefficients[1] *= norm;
176  coefficients[2] *= norm;
177  coefficients[5] *= norm;
178  coefficients[6] *= norm;
179  coefficients[7] *= norm;
180  }
181 #endif
182 }
183 
185 template <typename PointInT, typename PointOutT, typename NormalT> bool
186 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::initCompute ()
187 {
188  if (!Keypoint<PointInT, PointOutT>::initCompute ())
189  {
190  PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
191  return (false);
192  }
193 
194  if (method_ < 1 || method_ > 5)
195  {
196  PCL_ERROR ("[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
197  return (false);
198  }
199 
200  if (normals_->empty ())
201  {
202  normals_->reserve (surface_->size ());
203  if (input_->height == 1 ) // not organized
204  {
205  pcl::NormalEstimation<PointInT, NormalT> normal_estimation;
206  normal_estimation.setInputCloud(surface_);
207  normal_estimation.setRadiusSearch(search_radius_);
208  normal_estimation.compute (*normals_);
209  }
210  else
211  {
212  IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation;
213  normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT);
214  normal_estimation.setInputCloud(surface_);
215  normal_estimation.setNormalSmoothingSize (5.0);
216  normal_estimation.compute (*normals_);
217  }
218  }
219  return (true);
220 }
221 
223 template <typename PointInT, typename PointOutT, typename NormalT> void
224 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
225 {
226  boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
227 
228  response->points.reserve (input_->points.size());
229 
230  switch (method_)
231  {
232  case HARRIS:
233  responseHarris(*response);
234  break;
235  case NOBLE:
236  responseNoble(*response);
237  break;
238  case LOWE:
239  responseLowe(*response);
240  break;
241  case CURVATURE:
242  responseCurvature(*response);
243  break;
244  case TOMASI:
245  responseTomasi(*response);
246  break;
247  }
248 
249  if (!nonmax_)
250  output = *response;
251  else
252  {
253  output.points.clear ();
254  output.points.reserve (response->points.size());
255 
256 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
257 #pragma omp parallel for shared (output) num_threads(threads_)
258 #endif
259  for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
260  {
261  if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
262  continue;
263  std::vector<int> nn_indices;
264  std::vector<float> nn_dists;
265  tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
266  bool is_maxima = true;
267  for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
268  {
269  if (response->points[idx].intensity < response->points[*iIt].intensity)
270  {
271  is_maxima = false;
272  break;
273  }
274  }
275  if (is_maxima)
276 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
277 #pragma omp critical
278 #endif
279  output.points.push_back (response->points[idx]);
280  }
281 
282  if (refine_)
283  refineCorners (output);
284 
285  output.height = 1;
286  output.width = static_cast<uint32_t> (output.points.size());
287  }
288 
289  // we don not change the denseness
290  output.is_dense = input_->is_dense;
291 }
292 
294 template <typename PointInT, typename PointOutT, typename NormalT> void
295 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseHarris (PointCloudOut &output) const
296 {
297  PCL_ALIGN (16) float covar [8];
298  output.resize (input_->size ());
299 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
300  #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
301 #endif
302  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
303  {
304  const PointInT& pointIn = input_->points [pIdx];
305  output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
306  if (isFinite (pointIn))
307  {
308  std::vector<int> nn_indices;
309  std::vector<float> nn_dists;
310  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
311  calculateNormalCovar (nn_indices, covar);
312 
313  float trace = covar [0] + covar [5] + covar [7];
314  if (trace != 0)
315  {
316  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
317  - covar [2] * covar [2] * covar [5]
318  - covar [1] * covar [1] * covar [7]
319  - covar [6] * covar [6] * covar [0];
320 
321  output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
322  }
323  }
324  output [pIdx].x = pointIn.x;
325  output [pIdx].y = pointIn.y;
326  output [pIdx].z = pointIn.z;
327  }
328  output.height = input_->height;
329  output.width = input_->width;
330 }
331 
333 template <typename PointInT, typename PointOutT, typename NormalT> void
334 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseNoble (PointCloudOut &output) const
335 {
336  PCL_ALIGN (16) float covar [8];
337  output.resize (input_->size ());
338 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
339  #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
340 #endif
341  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
342  {
343  const PointInT& pointIn = input_->points [pIdx];
344  output [pIdx].intensity = 0.0;
345  if (isFinite (pointIn))
346  {
347  std::vector<int> nn_indices;
348  std::vector<float> nn_dists;
349  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
350  calculateNormalCovar (nn_indices, covar);
351  float trace = covar [0] + covar [5] + covar [7];
352  if (trace != 0)
353  {
354  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
355  - covar [2] * covar [2] * covar [5]
356  - covar [1] * covar [1] * covar [7]
357  - covar [6] * covar [6] * covar [0];
358 
359  output [pIdx].intensity = det / trace;
360  }
361  }
362  output [pIdx].x = pointIn.x;
363  output [pIdx].y = pointIn.y;
364  output [pIdx].z = pointIn.z;
365  }
366  output.height = input_->height;
367  output.width = input_->width;
368 }
369 
371 template <typename PointInT, typename PointOutT, typename NormalT> void
372 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseLowe (PointCloudOut &output) const
373 {
374  PCL_ALIGN (16) float covar [8];
375  output.resize (input_->size ());
376 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
377  #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
378 #endif
379  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
380  {
381  const PointInT& pointIn = input_->points [pIdx];
382  output [pIdx].intensity = 0.0;
383  if (isFinite (pointIn))
384  {
385  std::vector<int> nn_indices;
386  std::vector<float> nn_dists;
387  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
388  calculateNormalCovar (nn_indices, covar);
389  float trace = covar [0] + covar [5] + covar [7];
390  if (trace != 0)
391  {
392  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
393  - covar [2] * covar [2] * covar [5]
394  - covar [1] * covar [1] * covar [7]
395  - covar [6] * covar [6] * covar [0];
396 
397  output [pIdx].intensity = det / (trace * trace);
398  }
399  }
400  output [pIdx].x = pointIn.x;
401  output [pIdx].y = pointIn.y;
402  output [pIdx].z = pointIn.z;
403  }
404  output.height = input_->height;
405  output.width = input_->width;
406 }
407 
409 template <typename PointInT, typename PointOutT, typename NormalT> void
410 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseCurvature (PointCloudOut &output) const
411 {
412  PointOutT point;
413  for (unsigned idx = 0; idx < input_->points.size(); ++idx)
414  {
415  point.x = input_->points[idx].x;
416  point.y = input_->points[idx].y;
417  point.z = input_->points[idx].z;
418  point.intensity = normals_->points[idx].curvature;
419  output.points.push_back(point);
420  }
421  // does not change the order
422  output.height = input_->height;
423  output.width = input_->width;
424 }
425 
427 template <typename PointInT, typename PointOutT, typename NormalT> void
428 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const
429 {
430  PCL_ALIGN (16) float covar [8];
431  Eigen::Matrix3f covariance_matrix;
432  output.resize (input_->size ());
433 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
434  #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_)
435 #endif
436  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
437  {
438  const PointInT& pointIn = input_->points [pIdx];
439  output [pIdx].intensity = 0.0;
440  if (isFinite (pointIn))
441  {
442  std::vector<int> nn_indices;
443  std::vector<float> nn_dists;
444  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
445  calculateNormalCovar (nn_indices, covar);
446  float trace = covar [0] + covar [5] + covar [7];
447  if (trace != 0)
448  {
449  covariance_matrix.coeffRef (0) = covar [0];
450  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
451  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
452  covariance_matrix.coeffRef (4) = covar [5];
453  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
454  covariance_matrix.coeffRef (8) = covar [7];
455 
456  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
457  pcl::eigen33(covariance_matrix, eigen_values);
458  output [pIdx].intensity = eigen_values[0];
459  }
460  }
461  output [pIdx].x = pointIn.x;
462  output [pIdx].y = pointIn.y;
463  output [pIdx].z = pointIn.z;
464  }
465  output.height = input_->height;
466  output.width = input_->width;
467 }
468 
470 template <typename PointInT, typename PointOutT, typename NormalT> void
471 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const
472 {
473  Eigen::Matrix3f nnT;
474  Eigen::Matrix3f NNT;
475  Eigen::Matrix3f NNTInv;
476  Eigen::Vector3f NNTp;
477  float diff;
478  const unsigned max_iterations = 10;
479 #if defined (HAVE_OPENMP) && (defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2)))
480  #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_)
481 #endif
482  for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
483  {
484  unsigned iterations = 0;
485  do {
486  NNT.setZero();
487  NNTp.setZero();
488  PointInT corner;
489  corner.x = corners[cIdx].x;
490  corner.y = corners[cIdx].y;
491  corner.z = corners[cIdx].z;
492  std::vector<int> nn_indices;
493  std::vector<float> nn_dists;
494  tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
495  for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
496  {
497  if (!pcl_isfinite (normals_->points[*iIt].normal_x))
498  continue;
499 
500  nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
501  NNT += nnT;
502  NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
503  }
504  if (invert3x3SymMatrix (NNT, NNTInv) != 0)
505  corners[cIdx].getVector3fMap () = NNTInv * NNTp;
506 
507  diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
508  } while (diff > 1e-6 && ++iterations < max_iterations);
509  }
510 }
511 
512 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
513 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
514 
Generated on Fri Mar 8 2013 12:50:50 for Point Cloud Library (PCL) by   doxygen 1.8.3.1