Point Cloud Library (PCL)  1.7.0
harris_3d.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  */
37 
38 #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
39 #define PCL_HARRIS_KEYPOINT_3D_IMPL_H_
40 
41 #include <pcl/keypoints/harris_3d.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 HAVE_SSE_EXTENSIONS
50 #include <xmmintrin.h>
51 #endif
52 
53 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointInT, typename PointOutT, typename NormalT> void
56 {
57  method_ = method;
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointInT, typename PointOutT, typename NormalT> void
63 {
64  threshold_= threshold;
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointInT, typename PointOutT, typename NormalT> void
70 {
71  search_radius_ = radius;
72 }
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointInT, typename PointOutT, typename NormalT> void
77 {
78  refine_ = do_refine;
79 }
80 
81 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82 template <typename PointInT, typename PointOutT, typename NormalT> void
84 {
85  nonmax_ = nonmax;
86 }
87 
88 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89 template <typename PointInT, typename PointOutT, typename NormalT> void
91 {
92  normals_ = normals;
93 }
94 
95 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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 HAVE_SSE_EXTENSIONS
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 
184 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185 template <typename PointInT, typename PointOutT, typename NormalT> bool
187 {
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_)
201  {
202  PointCloudNPtr normals (new PointCloudN ());
203  normals->reserve (normals->size ());
204  if (!surface_->isOrganized ())
205  {
207  normal_estimation.setInputCloud (surface_);
208  normal_estimation.setRadiusSearch (search_radius_);
209  normal_estimation.compute (*normals);
210  }
211  else
212  {
215  normal_estimation.setInputCloud (surface_);
216  normal_estimation.setNormalSmoothingSize (5.0);
217  normal_estimation.compute (*normals);
218  }
219  normals_ = normals;
220  }
221  if (normals_->size () != surface_->size ())
222  {
223  PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_);
224  return (false);
225  }
226  return (true);
227 }
228 
229 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
230 template <typename PointInT, typename PointOutT, typename NormalT> void
232 {
233  boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
234 
235  response->points.reserve (input_->points.size());
236 
237  switch (method_)
238  {
239  case HARRIS:
240  responseHarris(*response);
241  break;
242  case NOBLE:
243  responseNoble(*response);
244  break;
245  case LOWE:
246  responseLowe(*response);
247  break;
248  case CURVATURE:
249  responseCurvature(*response);
250  break;
251  case TOMASI:
252  responseTomasi(*response);
253  break;
254  }
255 
256  if (!nonmax_)
257  {
258  output = *response;
259  // we do not change the denseness in this case
260  output.is_dense = input_->is_dense;
261  }
262  else
263  {
264  output.points.clear ();
265  output.points.reserve (response->points.size());
266 
267 #ifdef _OPENMP
268 #pragma omp parallel for shared (output) num_threads(threads_)
269 #endif
270  for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
271  {
272  if (!isFinite (response->points[idx]) ||
273  !pcl_isfinite (response->points[idx].intensity) ||
274  response->points[idx].intensity < threshold_)
275  continue;
276 
277  std::vector<int> nn_indices;
278  std::vector<float> nn_dists;
279  tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
280  bool is_maxima = true;
281  for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
282  {
283  if (response->points[idx].intensity < response->points[*iIt].intensity)
284  {
285  is_maxima = false;
286  break;
287  }
288  }
289  if (is_maxima)
290 #ifdef _OPENMP
291 #pragma omp critical
292 #endif
293  output.points.push_back (response->points[idx]);
294  }
295 
296  if (refine_)
297  refineCorners (output);
298 
299  output.height = 1;
300  output.width = static_cast<uint32_t> (output.points.size());
301  output.is_dense = true;
302  }
303 }
304 
305 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
306 template <typename PointInT, typename PointOutT, typename NormalT> void
308 {
309  PCL_ALIGN (16) float covar [8];
310  output.resize (input_->size ());
311 #ifdef _OPENMP
312  #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
313 #endif
314  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
315  {
316  const PointInT& pointIn = input_->points [pIdx];
317  output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
318  if (isFinite (pointIn))
319  {
320  std::vector<int> nn_indices;
321  std::vector<float> nn_dists;
322  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
323  calculateNormalCovar (nn_indices, covar);
324 
325  float trace = covar [0] + covar [5] + covar [7];
326  if (trace != 0)
327  {
328  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
329  - covar [2] * covar [2] * covar [5]
330  - covar [1] * covar [1] * covar [7]
331  - covar [6] * covar [6] * covar [0];
332 
333  output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
334  }
335  }
336  output [pIdx].x = pointIn.x;
337  output [pIdx].y = pointIn.y;
338  output [pIdx].z = pointIn.z;
339  }
340  output.height = input_->height;
341  output.width = input_->width;
342 }
343 
344 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
345 template <typename PointInT, typename PointOutT, typename NormalT> void
347 {
348  PCL_ALIGN (16) float covar [8];
349  output.resize (input_->size ());
350 #ifdef _OPENMP
351  #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
352 #endif
353  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
354  {
355  const PointInT& pointIn = input_->points [pIdx];
356  output [pIdx].intensity = 0.0;
357  if (isFinite (pointIn))
358  {
359  std::vector<int> nn_indices;
360  std::vector<float> nn_dists;
361  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
362  calculateNormalCovar (nn_indices, covar);
363  float trace = covar [0] + covar [5] + covar [7];
364  if (trace != 0)
365  {
366  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
367  - covar [2] * covar [2] * covar [5]
368  - covar [1] * covar [1] * covar [7]
369  - covar [6] * covar [6] * covar [0];
370 
371  output [pIdx].intensity = det / trace;
372  }
373  }
374  output [pIdx].x = pointIn.x;
375  output [pIdx].y = pointIn.y;
376  output [pIdx].z = pointIn.z;
377  }
378  output.height = input_->height;
379  output.width = input_->width;
380 }
381 
382 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
383 template <typename PointInT, typename PointOutT, typename NormalT> void
385 {
386  PCL_ALIGN (16) float covar [8];
387  output.resize (input_->size ());
388 #ifdef _OPENMP
389  #pragma omp parallel for shared (output) private (covar) num_threads(threads_)
390 #endif
391  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
392  {
393  const PointInT& pointIn = input_->points [pIdx];
394  output [pIdx].intensity = 0.0;
395  if (isFinite (pointIn))
396  {
397  std::vector<int> nn_indices;
398  std::vector<float> nn_dists;
399  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
400  calculateNormalCovar (nn_indices, covar);
401  float trace = covar [0] + covar [5] + covar [7];
402  if (trace != 0)
403  {
404  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
405  - covar [2] * covar [2] * covar [5]
406  - covar [1] * covar [1] * covar [7]
407  - covar [6] * covar [6] * covar [0];
408 
409  output [pIdx].intensity = det / (trace * trace);
410  }
411  }
412  output [pIdx].x = pointIn.x;
413  output [pIdx].y = pointIn.y;
414  output [pIdx].z = pointIn.z;
415  }
416  output.height = input_->height;
417  output.width = input_->width;
418 }
419 
420 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
421 template <typename PointInT, typename PointOutT, typename NormalT> void
423 {
424  PointOutT point;
425  for (unsigned idx = 0; idx < input_->points.size(); ++idx)
426  {
427  point.x = input_->points[idx].x;
428  point.y = input_->points[idx].y;
429  point.z = input_->points[idx].z;
430  point.intensity = normals_->points[idx].curvature;
431  output.points.push_back(point);
432  }
433  // does not change the order
434  output.height = input_->height;
435  output.width = input_->width;
436 }
437 
438 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
439 template <typename PointInT, typename PointOutT, typename NormalT> void
441 {
442  PCL_ALIGN (16) float covar [8];
443  Eigen::Matrix3f covariance_matrix;
444  output.resize (input_->size ());
445 #ifdef _OPENMP
446  #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_)
447 #endif
448  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
449  {
450  const PointInT& pointIn = input_->points [pIdx];
451  output [pIdx].intensity = 0.0;
452  if (isFinite (pointIn))
453  {
454  std::vector<int> nn_indices;
455  std::vector<float> nn_dists;
456  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
457  calculateNormalCovar (nn_indices, covar);
458  float trace = covar [0] + covar [5] + covar [7];
459  if (trace != 0)
460  {
461  covariance_matrix.coeffRef (0) = covar [0];
462  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
463  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
464  covariance_matrix.coeffRef (4) = covar [5];
465  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
466  covariance_matrix.coeffRef (8) = covar [7];
467 
468  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
469  pcl::eigen33(covariance_matrix, eigen_values);
470  output [pIdx].intensity = eigen_values[0];
471  }
472  }
473  output [pIdx].x = pointIn.x;
474  output [pIdx].y = pointIn.y;
475  output [pIdx].z = pointIn.z;
476  }
477  output.height = input_->height;
478  output.width = input_->width;
479 }
480 
481 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
482 template <typename PointInT, typename PointOutT, typename NormalT> void
484 {
485  Eigen::Matrix3f nnT;
486  Eigen::Matrix3f NNT;
487  Eigen::Matrix3f NNTInv;
488  Eigen::Vector3f NNTp;
489  float diff;
490  const unsigned max_iterations = 10;
491 #ifdef _OPENMP
492  #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_)
493 #endif
494  for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
495  {
496  unsigned iterations = 0;
497  do {
498  NNT.setZero();
499  NNTp.setZero();
500  PointInT corner;
501  corner.x = corners[cIdx].x;
502  corner.y = corners[cIdx].y;
503  corner.z = corners[cIdx].z;
504  std::vector<int> nn_indices;
505  std::vector<float> nn_dists;
506  tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
507  for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
508  {
509  if (!pcl_isfinite (normals_->points[*iIt].normal_x))
510  continue;
511 
512  nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
513  NNT += nnT;
514  NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
515  }
516  if (invert3x3SymMatrix (NNT, NNTInv) != 0)
517  corners[cIdx].getVector3fMap () = NNTInv * NNTp;
518 
519  diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
520  } while (diff > 1e-6 && ++iterations < max_iterations);
521  }
522 }
523 
524 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
525 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
526