Point Cloud Library (PCL)  1.14.0-dev
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/features/normal_3d.h>
44 #include <pcl/features/integral_image_normal.h>
45 #include <pcl/common/centroid.h>
46 #ifdef __SSE__
47 #include <xmmintrin.h>
48 #endif
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointInT, typename PointOutT, typename NormalT> void
53 {
54  if (normals_ && input_ && (cloud != input_))
55  normals_.reset ();
56  input_ = cloud;
57 }
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointInT, typename PointOutT, typename NormalT> void
62 {
63  method_ = method;
64 }
65 
66 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67 template <typename PointInT, typename PointOutT, typename NormalT> void
69 {
70  threshold_= threshold;
71 }
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74 template <typename PointInT, typename PointOutT, typename NormalT> void
76 {
77  search_radius_ = radius;
78 }
79 
80 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
81 template <typename PointInT, typename PointOutT, typename NormalT> void
83 {
84  refine_ = do_refine;
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
88 template <typename PointInT, typename PointOutT, typename NormalT> void
90 {
91  nonmax_ = nonmax;
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointInT, typename PointOutT, typename NormalT> void
97 {
98  normals_ = normals;
99 }
100 
101 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102 template <typename PointInT, typename PointOutT, typename NormalT> void
104 {
105  unsigned count = 0;
106  // indices 0 1 2 3 4 5 6 7
107  // coefficients: xx xy xz ?? yx yy yz ??
108 #ifdef __SSE__
109  // accumulator for xx, xy, xz
110  __m128 vec1 = _mm_setzero_ps();
111  // accumulator for yy, yz, zz
112  __m128 vec2 = _mm_setzero_ps();
113 
114  __m128 norm1;
115 
116  __m128 norm2;
117 
118  float zz = 0;
119 
120  for (const auto &neighbor : neighbors)
121  {
122  if (std::isfinite ((*normals_)[neighbor].normal_x))
123  {
124  // nx, ny, nz, h
125  norm1 = _mm_load_ps (&((*normals_)[neighbor].normal_x));
126 
127  // nx, nx, nx, nx
128  norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_x);
129 
130  // nx * nx, nx * ny, nx * nz, nx * h
131  norm2 = _mm_mul_ps (norm1, norm2);
132 
133  // accumulate
134  vec1 = _mm_add_ps (vec1, norm2);
135 
136  // ny, ny, ny, ny
137  norm2 = _mm_set1_ps ((*normals_)[neighbor].normal_y);
138 
139  // ny * nx, ny * ny, ny * nz, ny * h
140  norm2 = _mm_mul_ps (norm1, norm2);
141 
142  // accumulate
143  vec2 = _mm_add_ps (vec2, norm2);
144 
145  zz += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
146  ++count;
147  }
148  }
149  if (count > 0)
150  {
151  norm2 = _mm_set1_ps (static_cast<float>(count));
152  vec1 = _mm_div_ps (vec1, norm2);
153  vec2 = _mm_div_ps (vec2, norm2);
154  _mm_store_ps (coefficients, vec1);
155  _mm_store_ps (coefficients + 4, vec2);
156  coefficients [7] = zz / static_cast<float>(count);
157  }
158  else
159  std::fill_n(coefficients, 8, 0);
160 #else
161  std::fill_n(coefficients, 8, 0);
162  for (const auto& index : neighbors)
163  {
164  if (std::isfinite ((*normals_)[index].normal_x))
165  {
166  coefficients[0] += (*normals_)[index].normal_x * (*normals_)[index].normal_x;
167  coefficients[1] += (*normals_)[index].normal_x * (*normals_)[index].normal_y;
168  coefficients[2] += (*normals_)[index].normal_x * (*normals_)[index].normal_z;
169 
170  coefficients[5] += (*normals_)[index].normal_y * (*normals_)[index].normal_y;
171  coefficients[6] += (*normals_)[index].normal_y * (*normals_)[index].normal_z;
172  coefficients[7] += (*normals_)[index].normal_z * (*normals_)[index].normal_z;
173 
174  ++count;
175  }
176  }
177  if (count > 0)
178  {
179  float norm = 1.0 / float (count);
180  coefficients[0] *= norm;
181  coefficients[1] *= norm;
182  coefficients[2] *= norm;
183  coefficients[5] *= norm;
184  coefficients[6] *= norm;
185  coefficients[7] *= norm;
186  }
187 #endif
188 }
189 
190 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
191 template <typename PointInT, typename PointOutT, typename NormalT> bool
193 {
195  {
196  PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
197  return (false);
198  }
199 
200  if (method_ < 1 || method_ > 5)
201  {
202  PCL_ERROR ("[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_);
203  return (false);
204  }
205 
206  if (!normals_)
207  {
208  PointCloudNPtr normals (new PointCloudN ());
209  normals->reserve (normals->size ());
210  if (!surface_->isOrganized ())
211  {
213  normal_estimation.setInputCloud (surface_);
214  normal_estimation.setRadiusSearch (search_radius_);
215  normal_estimation.compute (*normals);
216  }
217  else
218  {
221  normal_estimation.setInputCloud (surface_);
222  normal_estimation.setNormalSmoothingSize (5.0);
223  normal_estimation.compute (*normals);
224  }
225  normals_ = normals;
226  }
227  if (normals_->size () != surface_->size ())
228  {
229  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_);
230  return (false);
231  }
232 
233  return (true);
234 }
235 
236 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
237 template <typename PointInT, typename PointOutT, typename NormalT> void
239 {
241 
242  response->points.reserve (input_->size());
243 
244  switch (method_)
245  {
246  case HARRIS:
247  responseHarris(*response);
248  break;
249  case NOBLE:
250  responseNoble(*response);
251  break;
252  case LOWE:
253  responseLowe(*response);
254  break;
255  case CURVATURE:
256  responseCurvature(*response);
257  break;
258  case TOMASI:
259  responseTomasi(*response);
260  break;
261  }
262 
263  if (!nonmax_)
264  {
265  output = *response;
266  // we do not change the denseness in this case
267  output.is_dense = input_->is_dense;
268  for (std::size_t i = 0; i < response->size (); ++i)
269  keypoints_indices_->indices.push_back (i);
270  }
271  else
272  {
273  output.clear ();
274  output.reserve (response->size());
275 
276 #pragma omp parallel for \
277  default(none) \
278  shared(output, response) \
279  num_threads(threads_)
280  for (int idx = 0; idx < static_cast<int> (response->size ()); ++idx)
281  {
282  if (!isFinite ((*response)[idx]) ||
283  !std::isfinite ((*response)[idx].intensity) ||
284  (*response)[idx].intensity < threshold_)
285  continue;
286 
287  pcl::Indices nn_indices;
288  std::vector<float> nn_dists;
289  tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
290  bool is_maxima = true;
291  for (const auto& index : nn_indices)
292  {
293  if ((*response)[idx].intensity < (*response)[index].intensity)
294  {
295  is_maxima = false;
296  break;
297  }
298  }
299  if (is_maxima)
300 #pragma omp critical
301  {
302  output.push_back ((*response)[idx]);
303  keypoints_indices_->indices.push_back (idx);
304  }
305  }
306 
307  if (refine_)
308  refineCorners (output);
309 
310  output.height = 1;
311  output.width = output.size();
312  output.is_dense = true;
313  }
314 }
315 
316 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
317 template <typename PointInT, typename PointOutT, typename NormalT> void
319 {
320  PCL_ALIGN (16) float covar [8];
321  output.resize (input_->size ());
322 #pragma omp parallel for \
323  default(none) \
324  shared(output) \
325  firstprivate(covar) \
326  num_threads(threads_)
327  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
328  {
329  const PointInT& pointIn = input_->points [pIdx];
330  output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
331  if (isFinite (pointIn))
332  {
333  pcl::Indices nn_indices;
334  std::vector<float> nn_dists;
335  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
336  calculateNormalCovar (nn_indices, covar);
337 
338  float trace = covar [0] + covar [5] + covar [7];
339  if (trace != 0)
340  {
341  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
342  - covar [2] * covar [2] * covar [5]
343  - covar [1] * covar [1] * covar [7]
344  - covar [6] * covar [6] * covar [0];
345 
346  output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
347  }
348  }
349  output [pIdx].x = pointIn.x;
350  output [pIdx].y = pointIn.y;
351  output [pIdx].z = pointIn.z;
352  }
353  output.height = input_->height;
354  output.width = input_->width;
355 }
356 
357 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
358 template <typename PointInT, typename PointOutT, typename NormalT> void
360 {
361  PCL_ALIGN (16) float covar [8];
362  output.resize (input_->size ());
363 #pragma omp parallel \
364  for default(none) \
365  shared(output) \
366  firstprivate(covar) \
367  num_threads(threads_)
368  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
369  {
370  const PointInT& pointIn = input_->points [pIdx];
371  output [pIdx].intensity = 0.0;
372  if (isFinite (pointIn))
373  {
374  pcl::Indices nn_indices;
375  std::vector<float> nn_dists;
376  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
377  calculateNormalCovar (nn_indices, covar);
378  float trace = covar [0] + covar [5] + covar [7];
379  if (trace != 0)
380  {
381  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
382  - covar [2] * covar [2] * covar [5]
383  - covar [1] * covar [1] * covar [7]
384  - covar [6] * covar [6] * covar [0];
385 
386  output [pIdx].intensity = det / trace;
387  }
388  }
389  output [pIdx].x = pointIn.x;
390  output [pIdx].y = pointIn.y;
391  output [pIdx].z = pointIn.z;
392  }
393  output.height = input_->height;
394  output.width = input_->width;
395 }
396 
397 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
398 template <typename PointInT, typename PointOutT, typename NormalT> void
400 {
401  PCL_ALIGN (16) float covar [8];
402  output.resize (input_->size ());
403 #pragma omp parallel for \
404  default(none) \
405  shared(output) \
406  firstprivate(covar) \
407  num_threads(threads_)
408  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
409  {
410  const PointInT& pointIn = input_->points [pIdx];
411  output [pIdx].intensity = 0.0;
412  if (isFinite (pointIn))
413  {
414  pcl::Indices nn_indices;
415  std::vector<float> nn_dists;
416  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
417  calculateNormalCovar (nn_indices, covar);
418  float trace = covar [0] + covar [5] + covar [7];
419  if (trace != 0)
420  {
421  float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
422  - covar [2] * covar [2] * covar [5]
423  - covar [1] * covar [1] * covar [7]
424  - covar [6] * covar [6] * covar [0];
425 
426  output [pIdx].intensity = det / (trace * trace);
427  }
428  }
429  output [pIdx].x = pointIn.x;
430  output [pIdx].y = pointIn.y;
431  output [pIdx].z = pointIn.z;
432  }
433  output.height = input_->height;
434  output.width = input_->width;
435 }
436 
437 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
438 template <typename PointInT, typename PointOutT, typename NormalT> void
440 {
441  PointOutT point;
442  for (std::size_t idx = 0; idx < input_->size(); ++idx)
443  {
444  point.x = (*input_)[idx].x;
445  point.y = (*input_)[idx].y;
446  point.z = (*input_)[idx].z;
447  point.intensity = (*normals_)[idx].curvature;
448  output.push_back(point);
449  }
450  // does not change the order
451  output.height = input_->height;
452  output.width = input_->width;
453 }
454 
455 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
456 template <typename PointInT, typename PointOutT, typename NormalT> void
458 {
459  PCL_ALIGN (16) float covar [8];
460  Eigen::Matrix3f covariance_matrix;
461  output.resize (input_->size ());
462 #pragma omp parallel for \
463  default(none) \
464  shared(output) \
465  firstprivate(covar, covariance_matrix) \
466  num_threads(threads_)
467  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
468  {
469  const PointInT& pointIn = input_->points [pIdx];
470  output [pIdx].intensity = 0.0;
471  if (isFinite (pointIn))
472  {
473  pcl::Indices nn_indices;
474  std::vector<float> nn_dists;
475  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
476  calculateNormalCovar (nn_indices, covar);
477  float trace = covar [0] + covar [5] + covar [7];
478  if (trace != 0)
479  {
480  covariance_matrix.coeffRef (0) = covar [0];
481  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
482  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
483  covariance_matrix.coeffRef (4) = covar [5];
484  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
485  covariance_matrix.coeffRef (8) = covar [7];
486 
487  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
488  pcl::eigen33(covariance_matrix, eigen_values);
489  output [pIdx].intensity = eigen_values[0];
490  }
491  }
492  output [pIdx].x = pointIn.x;
493  output [pIdx].y = pointIn.y;
494  output [pIdx].z = pointIn.z;
495  }
496  output.height = input_->height;
497  output.width = input_->width;
498 }
499 
500 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
501 template <typename PointInT, typename PointOutT, typename NormalT> void
503 {
504  Eigen::Matrix3f nnT;
505  Eigen::Matrix3f NNT;
506  Eigen::Vector3f NNTp;
507  constexpr unsigned max_iterations = 10;
508 #pragma omp parallel for \
509  shared(corners) \
510  firstprivate(nnT, NNT, NNTp) \
511  num_threads(threads_)
512  for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx)
513  {
514  unsigned iterations = 0;
515  do {
516  NNT.setZero();
517  NNTp.setZero();
518  PointInT corner;
519  corner.x = corners[cIdx].x;
520  corner.y = corners[cIdx].y;
521  corner.z = corners[cIdx].z;
522  pcl::Indices nn_indices;
523  std::vector<float> nn_dists;
524  tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
525  for (const auto& index : nn_indices)
526  {
527  if (!std::isfinite ((*normals_)[index].normal_x))
528  continue;
529 
530  nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();
531  NNT += nnT;
532  NNTp += nnT * (*surface_)[index].getVector3fMap ();
533  }
534  const Eigen::LDLT<Eigen::Matrix3f> ldlt(NNT);
535  if (ldlt.rcond() > 1e-4)
536  corners[cIdx].getVector3fMap () = ldlt.solve(NNTp);
537 
538  const auto diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
539  if (diff <= 1e-6) {
540  break;
541  }
542  } while (++iterations < max_iterations);
543  }
544 }
545 
546 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
547 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_
Define methods for centroid estimation and covariance matrix calculus.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:198
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
typename PointCloudN::Ptr PointCloudNPtr
Definition: harris_3d.h:63
void responseTomasi(PointCloudOut &output) const
Definition: harris_3d.hpp:457
void detectKeypoints(PointCloudOut &output) override
Definition: harris_3d.hpp:238
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: harris_3d.h:64
void responseNoble(PointCloudOut &output) const
Definition: harris_3d.hpp:359
bool initCompute() override
Definition: harris_3d.hpp:192
void responseHarris(PointCloudOut &output) const
gets the corner response for valid input points
Definition: harris_3d.hpp:318
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: harris_3d.h:58
void setNormals(const PointCloudNConstPtr &normals)
Set normals if precalculated normals are available.
Definition: harris_3d.hpp:96
void responseCurvature(PointCloudOut &output) const
Definition: harris_3d.hpp:439
void refineCorners(PointCloudOut &corners) const
Definition: harris_3d.hpp:502
void setRadius(float radius)
Set the radius for normal estimation and non maxima suppression.
Definition: harris_3d.hpp:75
void setNonMaxSupression(bool=false)
Whether non maxima suppression should be applied or the response for each point should be returned.
Definition: harris_3d.hpp:89
void setInputCloud(const PointCloudInConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: harris_3d.hpp:52
void setMethod(ResponseMethod type)
Set the method of the response to be calculated.
Definition: harris_3d.hpp:61
void setThreshold(float threshold)
Set the threshold value for detecting corners.
Definition: harris_3d.hpp:68
void responseLowe(PointCloudOut &output) const
Definition: harris_3d.hpp:399
void calculateNormalCovar(const pcl::Indices &neighbors, float *coefficients) const
calculates the upper triangular part of unnormalized covariance matrix over the normals given by the ...
Definition: harris_3d.hpp:103
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: harris_3d.h:60
void setRefine(bool do_refine)
Whether the detected key points should be refined or not.
Definition: harris_3d.hpp:82
Surface normal estimation on organized data using integral images.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Keypoint represents the base class for key points.
Definition: keypoint.h:49
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:244
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:328
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:403
std::size_t size() const
Definition: point_cloud.h:443
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:295
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133