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