Point Cloud Library (PCL)  1.12.0-dev
iss_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_ISS_KEYPOINT3D_IMPL_H_
39 #define PCL_ISS_KEYPOINT3D_IMPL_H_
40 
41 #include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
42 #include <pcl/features/boundary.h>
43 #include <pcl/features/normal_3d.h>
44 #include <pcl/features/integral_image_normal.h>
45 
46 #include <pcl/keypoints/iss_3d.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template<typename PointInT, typename PointOutT, typename NormalT> void
51 {
52  salient_radius_ = salient_radius;
53 }
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////
56 template<typename PointInT, typename PointOutT, typename NormalT> void
58 {
59  non_max_radius_ = non_max_radius;
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////
63 template<typename PointInT, typename PointOutT, typename NormalT> void
65 {
66  normal_radius_ = normal_radius;
67 }
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 template<typename PointInT, typename PointOutT, typename NormalT> void
72 {
73  border_radius_ = border_radius;
74 }
75 
76 //////////////////////////////////////////////////////////////////////////////////////////////
77 template<typename PointInT, typename PointOutT, typename NormalT> void
79 {
80  gamma_21_ = gamma_21;
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////
84 template<typename PointInT, typename PointOutT, typename NormalT> void
86 {
87  gamma_32_ = gamma_32;
88 }
89 
90 //////////////////////////////////////////////////////////////////////////////////////////////
91 template<typename PointInT, typename PointOutT, typename NormalT> void
93 {
94  min_neighbors_ = min_neighbors;
95 }
96 
97 //////////////////////////////////////////////////////////////////////////////////////////////
98 template<typename PointInT, typename PointOutT, typename NormalT> void
100 {
101  normals_ = normals;
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////////
105 template<typename PointInT, typename PointOutT, typename NormalT> void
107 {
108  if (nr_threads == 0)
109 #ifdef _OPENMP
110  threads_ = omp_get_num_procs();
111 #else
112  threads_ = 1;
113 #endif
114  else
115  threads_ = nr_threads;
116 }
117 
118 //////////////////////////////////////////////////////////////////////////////////////////////
119 template<typename PointInT, typename PointOutT, typename NormalT> bool*
120 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getBoundaryPoints (PointCloudIn &input, double border_radius, float angle_threshold)
121 {
122  bool* edge_points = new bool [input.size ()];
123 
124  Eigen::Vector4f u = Eigen::Vector4f::Zero ();
125  Eigen::Vector4f v = Eigen::Vector4f::Zero ();
126 
128  boundary_estimator.setInputCloud (input_);
129 
130 #pragma omp parallel for \
131  default(none) \
132  shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \
133  firstprivate(u, v) \
134  num_threads(threads_)
135  for (int index = 0; index < int (input.size ()); index++)
136  {
137  edge_points[index] = false;
138  PointInT current_point = input[index];
139 
140  if (pcl::isFinite(current_point))
141  {
142  pcl::Indices nn_indices;
143  std::vector<float> nn_distances;
144  int n_neighbors;
145 
146  this->searchForNeighbors (index, border_radius, nn_indices, nn_distances);
147 
148  n_neighbors = static_cast<int> (nn_indices.size ());
149 
150  if (n_neighbors >= min_neighbors_)
151  {
152  boundary_estimator.getCoordinateSystemOnPlane ((*normals_)[index], u, v);
153 
154  if (boundary_estimator.isBoundaryPoint (input, static_cast<int> (index), nn_indices, u, v, angle_threshold))
155  edge_points[index] = true;
156  }
157  }
158  }
159 
160  return (edge_points);
161 }
162 
163 //////////////////////////////////////////////////////////////////////////////////////////////
164 template<typename PointInT, typename PointOutT, typename NormalT> void
165 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getScatterMatrix (const int& current_index, Eigen::Matrix3d &cov_m)
166 {
167  const PointInT& current_point = (*input_)[current_index];
168 
169  double central_point[3];
170  memset(central_point, 0, sizeof(double) * 3);
171 
172  central_point[0] = current_point.x;
173  central_point[1] = current_point.y;
174  central_point[2] = current_point.z;
175 
176  cov_m = Eigen::Matrix3d::Zero ();
177 
178  pcl::Indices nn_indices;
179  std::vector<float> nn_distances;
180  int n_neighbors;
181 
182  this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
183 
184  n_neighbors = static_cast<int> (nn_indices.size ());
185 
186  if (n_neighbors < min_neighbors_)
187  return;
188 
189  double cov[9];
190  memset(cov, 0, sizeof(double) * 9);
191 
192  for (const auto& n_idx : nn_indices)
193  {
194  const PointInT& n_point = (*input_)[n_idx];
195 
196  double neigh_point[3];
197  memset(neigh_point, 0, sizeof(double) * 3);
198 
199  neigh_point[0] = n_point.x;
200  neigh_point[1] = n_point.y;
201  neigh_point[2] = n_point.z;
202 
203  for (int i = 0; i < 3; i++)
204  for (int j = 0; j < 3; j++)
205  cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
206  }
207 
208  cov_m << cov[0], cov[1], cov[2],
209  cov[3], cov[4], cov[5],
210  cov[6], cov[7], cov[8];
211 }
212 
213 //////////////////////////////////////////////////////////////////////////////////////////////
214 template<typename PointInT, typename PointOutT, typename NormalT> bool
216 {
218  {
219  PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
220  return (false);
221  }
222  if (salient_radius_ <= 0)
223  {
224  PCL_ERROR ("[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
225  name_.c_str (), salient_radius_);
226  return (false);
227  }
228  if (non_max_radius_ <= 0)
229  {
230  PCL_ERROR ("[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
231  name_.c_str (), non_max_radius_);
232  return (false);
233  }
234  if (gamma_21_ <= 0)
235  {
236  PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
237  name_.c_str (), gamma_21_);
238  return (false);
239  }
240  if (gamma_32_ <= 0)
241  {
242  PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
243  name_.c_str (), gamma_32_);
244  return (false);
245  }
246  if (min_neighbors_ <= 0)
247  {
248  PCL_ERROR ("[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
249  name_.c_str (), min_neighbors_);
250  return (false);
251  }
252 
253  delete[] third_eigen_value_;
254 
255  third_eigen_value_ = new double[input_->size ()];
256  memset(third_eigen_value_, 0, sizeof(double) * input_->size ());
257 
258  delete[] edge_points_;
259 
260  if (border_radius_ > 0.0)
261  {
262  if (normals_->empty ())
263  {
264  if (normal_radius_ <= 0.)
265  {
266  PCL_ERROR ("[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
267  name_.c_str (), normal_radius_);
268  return (false);
269  }
270 
271  PointCloudNPtr normal_ptr (new PointCloudN ());
272  if (input_->height == 1 )
273  {
275  normal_estimation.setInputCloud (surface_);
276  normal_estimation.setRadiusSearch (normal_radius_);
277  normal_estimation.compute (*normal_ptr);
278  }
279  else
280  {
283  normal_estimation.setInputCloud (surface_);
284  normal_estimation.setNormalSmoothingSize (5.0);
285  normal_estimation.compute (*normal_ptr);
286  }
287  normals_ = normal_ptr;
288  }
289  if (normals_->size () != surface_->size ())
290  {
291  PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
292  return (false);
293  }
294  }
295  else if (border_radius_ < 0.0)
296  {
297  PCL_ERROR ("[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
298  name_.c_str (), border_radius_);
299  return (false);
300  }
301 
302  return (true);
303 }
304 
305 //////////////////////////////////////////////////////////////////////////////////////////////
306 template<typename PointInT, typename PointOutT, typename NormalT> void
308 {
309  // Make sure the output cloud is empty
310  output.clear ();
311 
312  if (border_radius_ > 0.0)
313  edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
314 
315  bool* borders = new bool [input_->size()];
316 
317 #pragma omp parallel for \
318  default(none) \
319  shared(borders) \
320  num_threads(threads_)
321  for (int index = 0; index < int (input_->size ()); index++)
322  {
323  borders[index] = false;
324  PointInT current_point = (*input_)[index];
325 
326  if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
327  {
328  pcl::Indices nn_indices;
329  std::vector<float> nn_distances;
330 
331  this->searchForNeighbors (index, border_radius_, nn_indices, nn_distances);
332 
333  for (const auto &nn_index : nn_indices)
334  {
335  if (edge_points_[nn_index])
336  {
337  borders[index] = true;
338  break;
339  }
340  }
341  }
342  }
343 
344 #ifdef _OPENMP
345  Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
346 
347  for (std::size_t i = 0; i < threads_; i++)
348  omp_mem[i].setZero (3);
349 #else
350  Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1];
351 
352  omp_mem[0].setZero (3);
353 #endif
354 
355  double *prg_local_mem = new double[input_->size () * 3];
356  double **prg_mem = new double * [input_->size ()];
357 
358  for (std::size_t i = 0; i < input_->size (); i++)
359  prg_mem[i] = prg_local_mem + 3 * i;
360 
361 #pragma omp parallel for \
362  default(none) \
363  shared(borders, omp_mem, prg_mem) \
364  num_threads(threads_)
365  for (int index = 0; index < static_cast<int> (input_->size ()); index++)
366  {
367 #ifdef _OPENMP
368  int tid = omp_get_thread_num ();
369 #else
370  int tid = 0;
371 #endif
372  PointInT current_point = (*input_)[index];
373 
374  if ((!borders[index]) && pcl::isFinite(current_point))
375  {
376  //if the considered point is not a border point and the point is "finite", then compute the scatter matrix
377  Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
378  getScatterMatrix (static_cast<int> (index), cov_m);
379 
380  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
381 
382  const double& e1c = solver.eigenvalues ()[2];
383  const double& e2c = solver.eigenvalues ()[1];
384  const double& e3c = solver.eigenvalues ()[0];
385 
386  if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c))
387  continue;
388 
389  if (e3c < 0)
390  {
391  PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
392  name_.c_str (), index);
393  continue;
394  }
395 
396  omp_mem[tid][0] = e2c / e1c;
397  omp_mem[tid][1] = e3c / e2c;;
398  omp_mem[tid][2] = e3c;
399  }
400 
401  for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++)
402  prg_mem[index][d] = omp_mem[tid][d];
403  }
404 
405  for (int index = 0; index < int (input_->size ()); index++)
406  {
407  if (!borders[index])
408  {
409  if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
410  third_eigen_value_[index] = prg_mem[index][2];
411  }
412  }
413 
414  bool* feat_max = new bool [input_->size()];
415 
416 #pragma omp parallel for \
417  default(none) \
418  shared(feat_max) \
419  num_threads(threads_)
420  for (int index = 0; index < int (input_->size ()); index++)
421  {
422  feat_max [index] = false;
423  PointInT current_point = (*input_)[index];
424 
425  if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
426  {
427  pcl::Indices nn_indices;
428  std::vector<float> nn_distances;
429  int n_neighbors;
430 
431  this->searchForNeighbors (index, non_max_radius_, nn_indices, nn_distances);
432 
433  n_neighbors = static_cast<int> (nn_indices.size ());
434 
435  if (n_neighbors >= min_neighbors_)
436  {
437  bool is_max = true;
438 
439  for (const auto& j : nn_indices)
440  if (third_eigen_value_[index] < third_eigen_value_[j])
441  is_max = false;
442  if (is_max)
443  feat_max[index] = true;
444  }
445  }
446  }
447 
448 #pragma omp parallel for \
449  default(none) \
450  shared(feat_max, output) \
451  num_threads(threads_)
452  for (int index = 0; index < int (input_->size ()); index++)
453  {
454  if (feat_max[index])
455 #pragma omp critical
456  {
457  PointOutT p;
458  p.getVector3fMap () = (*input_)[index].getVector3fMap ();
459  output.push_back(p);
460  keypoints_indices_->indices.push_back (index);
461  }
462  }
463 
464  output.header = input_->header;
465  output.width = output.size ();
466  output.height = 1;
467 
468  // Clear the contents of variables and arrays before the beginning of the next computation.
469  if (border_radius_ > 0.0)
470  normals_.reset (new pcl::PointCloud<NormalT>);
471 
472  delete[] borders;
473  delete[] prg_mem;
474  delete[] prg_local_mem;
475  delete[] feat_max;
476  delete[] omp_mem;
477 }
478 
479 #define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
480 
481 #endif /* PCL_ISS_3D_IMPL_H_ */
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:243
pcl::isFinite
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
pcl::ISSKeypoint3D::PointCloudNConstPtr
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: iss_3d.h:96
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition: integral_image_normal.h:65
pcl::ISSKeypoint3D::setNumberOfThreads
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition: iss_3d.hpp:106
pcl::ISSKeypoint3D::detectKeypoints
void detectKeypoints(PointCloudOut &output) override
Detect the keypoints by performing the EVD of the scatter matrix.
Definition: iss_3d.hpp:307
pcl::ISSKeypoint3D::getBoundaryPoints
bool * getBoundaryPoints(PointCloudIn &input, double border_radius, float angle_threshold)
Compute the boundary points for the given input cloud.
Definition: iss_3d.hpp:120
pcl::ISSKeypoint3D::setThreshold32
void setThreshold32(double gamma_32)
Set the upper bound on the ratio between the third and the second eigenvalue.
Definition: iss_3d.hpp:85
pcl::ISSKeypoint3D::initCompute
bool initCompute() override
Perform the initial checks before computing the keypoints.
Definition: iss_3d.hpp:215
pcl::PointCloud< NormalT >
pcl::IntegralImageNormalEstimation::setInputCloud
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
Definition: integral_image_normal.h:232
pcl::IntegralImageNormalEstimation::setNormalEstimationMethod
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
Definition: integral_image_normal.h:214
pcl::PCLBase< PointInT >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
pcl::BoundaryEstimation
BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle cr...
Definition: boundary.h:79
pcl::ISSKeypoint3D::getScatterMatrix
void getScatterMatrix(const int &current_index, Eigen::Matrix3d &cov_m)
Compute the scatter matrix for a point index.
Definition: iss_3d.hpp:165
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
pcl::BoundaryEstimation::getCoordinateSystemOnPlane
void getCoordinateSystemOnPlane(const PointNT &p_coeff, Eigen::Vector4f &u, Eigen::Vector4f &v)
Get a u-v-n coordinate system that lies on a plane defined by its normal.
Definition: boundary.h:159
pcl::IntegralImageNormalEstimation::setNormalSmoothingSize
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Definition: integral_image_normal.h:190
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
pcl::ISSKeypoint3D::setNormals
void setNormals(const PointCloudNConstPtr &normals)
Set the normals if pre-calculated normals are available.
Definition: iss_3d.hpp:99
pcl::ISSKeypoint3D::PointCloudIn
typename Keypoint< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition: iss_3d.h:91
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::ISSKeypoint3D::setNormalRadius
void setNormalRadius(double normal_radius)
Set the radius used for the estimation of the surface normals of the input cloud.
Definition: iss_3d.hpp:64
pcl::ISSKeypoint3D::setThreshold21
void setThreshold21(double gamma_21)
Set the upper bound on the ratio between the second and the first eigenvalue.
Definition: iss_3d.hpp:78
pcl::ISSKeypoint3D::PointCloudOut
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: iss_3d.h:92
pcl::Feature::setRadiusSearch
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:201
pcl::Keypoint
Keypoint represents the base class for key points.
Definition: keypoint.h:49
pcl::ISSKeypoint3D::setNonMaxRadius
void setNonMaxRadius(double non_max_radius)
Set the radius for the application of the non maxima supression algorithm.
Definition: iss_3d.hpp:57
pcl::BoundaryEstimation::isBoundaryPoint
bool isBoundaryPoint(const pcl::PointCloud< PointInT > &cloud, int q_idx, const pcl::Indices &indices, const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold)
Check whether a point is a boundary point in a planar patch of projected points given by indices.
Definition: boundary.hpp:51
pcl::ISSKeypoint3D::setBorderRadius
void setBorderRadius(double border_radius)
Set the radius used for the estimation of the boundary points.
Definition: iss_3d.hpp:71
pcl::ISSKeypoint3D::setSalientRadius
void setSalientRadius(double salient_radius)
Set the radius of the spherical neighborhood used to compute the scatter matrix.
Definition: iss_3d.hpp:50
pcl::ISSKeypoint3D::PointCloudNPtr
typename PointCloudN::Ptr PointCloudNPtr
Definition: iss_3d.h:95
pcl::ISSKeypoint3D::setMinNeighbors
void setMinNeighbors(int min_neighbors)
Set the minimum number of neighbors that has to be found while applying the non maxima suppression al...
Definition: iss_3d.hpp:92