Point Cloud Library (PCL)  1.7.1
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  int index;
116 #ifdef _OPENMP
117 #pragma omp parallel for private(u, v) num_threads(threads_)
118 #endif
119  for (index = 0; index < int (input.points.size ()); index++)
120  {
121  edge_points[index] = false;
122  PointInT current_point = input.points[index];
123 
124  if (pcl::isFinite(current_point))
125  {
126  std::vector<int> nn_indices;
127  std::vector<float> nn_distances;
128  int n_neighbors;
129 
130  this->searchForNeighbors (static_cast<int> (index), border_radius, nn_indices, nn_distances);
131 
132  n_neighbors = static_cast<int> (nn_indices.size ());
133 
134  if (n_neighbors >= min_neighbors_)
135  {
136  boundary_estimator.getCoordinateSystemOnPlane (normals_->points[index], u, v);
137 
138  if (boundary_estimator.isBoundaryPoint (input, static_cast<int> (index), nn_indices, u, v, angle_threshold))
139  edge_points[index] = true;
140  }
141  }
142  }
143 
144  return (edge_points);
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////
148 template<typename PointInT, typename PointOutT, typename NormalT> void
149 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getScatterMatrix (const int& current_index, Eigen::Matrix3d &cov_m)
150 {
151  const PointInT& current_point = (*input_).points[current_index];
152 
153  double central_point[3];
154  memset(central_point, 0, sizeof(double) * 3);
155 
156  central_point[0] = current_point.x;
157  central_point[1] = current_point.y;
158  central_point[2] = current_point.z;
159 
160  cov_m = Eigen::Matrix3d::Zero ();
161 
162  std::vector<int> nn_indices;
163  std::vector<float> nn_distances;
164  int n_neighbors;
165 
166  this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
167 
168  n_neighbors = static_cast<int> (nn_indices.size ());
169 
170  if (n_neighbors < min_neighbors_)
171  return;
172 
173  double cov[9];
174  memset(cov, 0, sizeof(double) * 9);
175 
176  for (size_t n_idx = 0; n_idx < n_neighbors; n_idx++)
177  {
178  const PointInT& n_point = (*input_).points[nn_indices[n_idx]];
179 
180  double neigh_point[3];
181  memset(neigh_point, 0, sizeof(double) * 3);
182 
183  neigh_point[0] = n_point.x;
184  neigh_point[1] = n_point.y;
185  neigh_point[2] = n_point.z;
186 
187  for (int i = 0; i < 3; i++)
188  for (int j = 0; j < 3; j++)
189  cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
190  }
191 
192  cov_m << cov[0], cov[1], cov[2],
193  cov[3], cov[4], cov[5],
194  cov[6], cov[7], cov[8];
195 }
196 
197 //////////////////////////////////////////////////////////////////////////////////////////////
198 template<typename PointInT, typename PointOutT, typename NormalT> bool
200 {
202  {
203  PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
204  return (false);
205  }
206  if (salient_radius_ <= 0)
207  {
208  PCL_ERROR ("[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
209  name_.c_str (), salient_radius_);
210  return (false);
211  }
212  if (non_max_radius_ <= 0)
213  {
214  PCL_ERROR ("[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
215  name_.c_str (), non_max_radius_);
216  return (false);
217  }
218  if (gamma_21_ <= 0)
219  {
220  PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
221  name_.c_str (), gamma_21_);
222  return (false);
223  }
224  if (gamma_32_ <= 0)
225  {
226  PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
227  name_.c_str (), gamma_32_);
228  return (false);
229  }
230  if (min_neighbors_ <= 0)
231  {
232  PCL_ERROR ("[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
233  name_.c_str (), min_neighbors_);
234  return (false);
235  }
236 
237  if (third_eigen_value_)
238  delete[] third_eigen_value_;
239 
240  third_eigen_value_ = new double[input_->size ()];
241  memset(third_eigen_value_, 0, sizeof(double) * input_->size ());
242 
243  if (edge_points_)
244  delete[] edge_points_;
245 
246  if (border_radius_ > 0.0)
247  {
248  if (normals_->empty ())
249  {
250  if (normal_radius_ <= 0.)
251  {
252  PCL_ERROR ("[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
253  name_.c_str (), normal_radius_);
254  return (false);
255  }
256 
257  PointCloudNPtr normal_ptr (new PointCloudN ());
258  if (input_->height == 1 )
259  {
261  normal_estimation.setInputCloud (surface_);
262  normal_estimation.setRadiusSearch (normal_radius_);
263  normal_estimation.compute (*normal_ptr);
264  }
265  else
266  {
269  normal_estimation.setInputCloud (surface_);
270  normal_estimation.setNormalSmoothingSize (5.0);
271  normal_estimation.compute (*normal_ptr);
272  }
273  normals_ = normal_ptr;
274  }
275  if (normals_->size () != surface_->size ())
276  {
277  PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
278  return (false);
279  }
280  }
281  else if (border_radius_ < 0.0)
282  {
283  PCL_ERROR ("[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
284  name_.c_str (), border_radius_);
285  return (false);
286  }
287 
288  return (true);
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////
292 template<typename PointInT, typename PointOutT, typename NormalT> void
294 {
295  // Make sure the output cloud is empty
296  output.points.clear ();
297 
298  if (border_radius_ > 0.0)
299  edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
300 
301  bool* borders = new bool [input_->size()];
302 
303  int index;
304 #ifdef _OPENMP
305  #pragma omp parallel for num_threads(threads_)
306 #endif
307  for (index = 0; index < int (input_->size ()); index++)
308  {
309  borders[index] = false;
310  PointInT current_point = input_->points[index];
311 
312  if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
313  {
314  std::vector<int> nn_indices;
315  std::vector<float> nn_distances;
316 
317  this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances);
318 
319  for (int j = 0 ; j < nn_indices.size (); j++)
320  {
321  if (edge_points_[nn_indices[j]])
322  {
323  borders[index] = true;
324  break;
325  }
326  }
327  }
328  }
329 
330  Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
331 
332  for (int i = 0; i < threads_; i++)
333  omp_mem[i].setZero (3);
334 
335  double *prg_local_mem = new double[input_->size () * 3];
336  double **prg_mem = new double * [input_->size ()];
337 
338  for (int i = 0; i < input_->size (); i++)
339  prg_mem[i] = prg_local_mem + 3 * i;
340 
341 #ifdef _OPENMP
342  #pragma omp parallel for num_threads(threads_)
343 #endif
344  for (index = 0; index < static_cast<int> (input_->size ()); index++)
345  {
346 #ifdef _OPENMP
347  int tid = omp_get_thread_num ();
348 #else
349  int tid = 0;
350 #endif
351  PointInT current_point = input_->points[index];
352 
353  if ((!borders[index]) && pcl::isFinite(current_point))
354  {
355  //if the considered point is not a border point and the point is "finite", then compute the scatter matrix
356  Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
357  getScatterMatrix (static_cast<int> (index), cov_m);
358 
359  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
360 
361  const double& e1c = solver.eigenvalues ()[2];
362  const double& e2c = solver.eigenvalues ()[1];
363  const double& e3c = solver.eigenvalues ()[0];
364 
365  if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
366  continue;
367 
368  if (e3c < 0)
369  {
370  PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
371  name_.c_str (), index);
372  continue;
373  }
374 
375  omp_mem[tid][0] = e2c / e1c;
376  omp_mem[tid][1] = e3c / e2c;;
377  omp_mem[tid][2] = e3c;
378  }
379 
380  for (int d = 0; d < omp_mem[tid].size (); d++)
381  prg_mem[index][d] = omp_mem[tid][d];
382  }
383 
384  for (index = 0; index < int (input_->size ()); index++)
385  {
386  if (!borders[index])
387  {
388  if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
389  third_eigen_value_[index] = prg_mem[index][2];
390  }
391  }
392 
393  bool* feat_max = new bool [input_->size()];
394  bool is_max;
395 
396 #ifdef _OPENMP
397  #pragma omp parallel for private(is_max) num_threads(threads_)
398 #endif
399  for (index = 0; index < int (input_->size ()); index++)
400  {
401  feat_max [index] = false;
402  PointInT current_point = input_->points[index];
403 
404  if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
405  {
406  std::vector<int> nn_indices;
407  std::vector<float> nn_distances;
408  int n_neighbors;
409 
410  this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
411 
412  n_neighbors = static_cast<int> (nn_indices.size ());
413 
414  if (n_neighbors >= min_neighbors_)
415  {
416  is_max = true;
417 
418  for (size_t j = 0 ; j < n_neighbors; j++)
419  if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
420  is_max = false;
421  if (is_max)
422  feat_max[index] = true;
423  }
424  }
425  }
426 
427 #ifdef _OPENMP
428 #pragma omp parallel for shared (output) num_threads(threads_)
429 #endif
430  for (index = 0; index < int (input_->size ()); index++)
431  {
432  if (feat_max[index])
433 #ifdef _OPENMP
434 #pragma omp critical
435 #endif
436  output.points.push_back(input_->points[index]);
437  }
438 
439  output.header = input_->header;
440  output.width = static_cast<uint32_t> (output.points.size ());
441  output.height = 1;
442 
443  // Clear the contents of variables and arrays before the beginning of the next computation.
444  if (border_radius_ > 0.0)
445  normals_.reset (new pcl::PointCloud<NormalT>);
446 
447  delete[] borders;
448  delete[] prg_mem;
449  delete[] prg_local_mem;
450  delete[] feat_max;
451 }
452 
453 #define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
454 
455 #endif /* PCL_ISS_3D_IMPL_H_ */