Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
shot_omp.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  *
35  */
36 
37 #ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_
38 #define PCL_FEATURES_IMPL_SHOT_OMP_H_
39 
40 #include <pcl/features/shot_omp.h>
41 #include <pcl/common/time.h>
42 #include <pcl/features/shot_lrf_omp.h>
43 
44 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
45 pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
46 {
47  if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
48  {
49  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
50  return (false);
51  }
52 
53  // SHOT cannot work with k-search
54  if (this->getKSearch () != 0)
55  {
56  PCL_ERROR(
57  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
58  getClassName().c_str ());
59  return (false);
60  }
61 
62  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
63  typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>());
64  lrf_estimator->setRadiusSearch (search_radius_);
65  lrf_estimator->setInputCloud (input_);
66  lrf_estimator->setIndices (indices_);
67  lrf_estimator->setNumberOfThreads(threads_);
68 
69  if (!fake_surface_)
70  lrf_estimator->setSearchSurface(surface_);
71 
72  if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
73  {
74  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
75  return (false);
76  }
77 
78  return (true);
79 }
80 
82 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
83 pcl::SHOTColorEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
84 {
85  if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
86  {
87  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
88  return (false);
89  }
90 
91  // SHOT cannot work with k-search
92  if (this->getKSearch () != 0)
93  {
94  PCL_ERROR(
95  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
96  getClassName().c_str ());
97  return (false);
98  }
99 
100  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
101  typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>());
102  lrf_estimator->setRadiusSearch (search_radius_);
103  lrf_estimator->setInputCloud (input_);
104  lrf_estimator->setIndices (indices_);
105  lrf_estimator->setNumberOfThreads(threads_);
106 
107  if (!fake_surface_)
108  lrf_estimator->setSearchSurface(surface_);
109 
110  if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
111  {
112  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
113  return (false);
114  }
115 
116  return (true);
117 }
118 
120 template<typename PointInT, typename PointNT, typename PointRFT> bool
121 pcl::SHOTEstimationOMP<PointInT, PointNT, pcl::SHOT, PointRFT>::initCompute ()
122 {
123  if (!FeatureFromNormals<PointInT, PointNT, pcl::SHOT>::initCompute ())
124  {
125  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
126  return (false);
127  }
128 
129  // SHOT cannot work with k-search
130  if (this->getKSearch () != 0)
131  {
132  PCL_ERROR(
133  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
134  getClassName().c_str ());
135  return (false);
136  }
137 
138  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
139  typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>());
140  lrf_estimator->setRadiusSearch (search_radius_);
141  lrf_estimator->setInputCloud (input_);
142  lrf_estimator->setIndices (indices_);
143  lrf_estimator->setNumberOfThreads(threads_);
144 
145  if (!fake_surface_)
146  lrf_estimator->setSearchSurface(surface_);
147 
148  if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
149  {
150  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
151  return (false);
152  }
153 
154  return (true);
155 }
156 
158 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
159 pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
160 {
161 
162  if (threads_ < 0)
163  threads_ = 1;
164 
165  descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
166 
167  sqradius_ = search_radius_ * search_radius_;
168  radius3_4_ = (search_radius_ * 3) / 4;
169  radius1_4_ = search_radius_ / 4;
170  radius1_2_ = search_radius_ / 2;
171 
172  assert(descLength_ == 352);
173 
174  int data_size = static_cast<int> (indices_->size ());
175  Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
176 
177  for (int i = 0; i < threads_; i++)
178  shot[i].setZero (descLength_);
179 
180  output.is_dense = true;
181  // Iterating over the entire index vector
182  #pragma omp parallel for num_threads(threads_)
183  for (int idx = 0; idx < data_size; ++idx)
184  {
185 #ifdef _OPENMP
186  int tid = omp_get_thread_num ();
187 #else
188  int tid = 0;
189 #endif
190 
191  bool lrf_is_nan = false;
192  const PointRFT& current_frame = (*frames_)[idx];
193  if (!pcl_isfinite (current_frame.rf[0]) ||
194  !pcl_isfinite (current_frame.rf[4]) ||
195  !pcl_isfinite (current_frame.rf[11]))
196  {
197  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
198  getClassName ().c_str (), (*indices_)[idx]);
199  lrf_is_nan = true;
200  }
201 
202  // Allocate enough space to hold the results
203  // \note This resize is irrelevant for a radiusSearch ().
204  std::vector<int> nn_indices (k_);
205  std::vector<float> nn_dists (k_);
206 
207  if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
208  nn_dists) == 0)
209  {
210  // Copy into the resultant cloud
211  for (int d = 0; d < shot[tid].size (); ++d)
212  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
213  for (int d = 0; d < 9; ++d)
214  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
215 
216  output.is_dense = false;
217  continue;
218  }
219 
220  // Estimate the SHOT at each patch
221  this->computePointSHOT (idx, nn_indices, nn_dists, shot[tid]);
222 
223  // Copy into the resultant cloud
224  for (int d = 0; d < shot[tid].size (); ++d)
225  output.points[idx].descriptor[d] = shot[tid][d];
226  for (int d = 0; d < 9; ++d)
227  output.points[idx].rf[d] = frames_->points[idx].rf[(4 * (d / 3) + (d % 3))];
228  }
229  delete[] shot;
230 }
231 
233 template<typename PointInT, typename PointNT, typename PointRFT> void
234 pcl::SHOTEstimationOMP<PointInT, PointNT, pcl::SHOT, PointRFT>::computeFeature (PointCloudOut &output)
235 {
236 
237  if (threads_ < 0)
238  threads_ = 1;
239 
240  descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
241 
242  sqradius_ = search_radius_ * search_radius_;
243  radius3_4_ = (search_radius_ * 3) / 4;
244  radius1_4_ = search_radius_ / 4;
245  radius1_2_ = search_radius_ / 2;
246 
247  for (size_t idx = 0; idx < indices_->size (); ++idx)
248  output.points[idx].descriptor.resize (descLength_);
249 
250  int data_size = static_cast<int> (indices_->size ());
251  Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
252 
253  for (int i = 0; i < threads_; i++)
254  shot[i].setZero (descLength_);
255 
256  output.is_dense = true;
257  // Iterating over the entire index vector
258  #pragma omp parallel for num_threads(threads_)
259  for (int idx = 0; idx < data_size; ++idx)
260  {
261 #ifdef _OPENMP
262  int tid = omp_get_thread_num ();
263 #else
264  int tid = 0;
265 #endif
266 
267  bool lrf_is_nan = false;
268  const PointRFT& current_frame = (*frames_)[idx];
269  if (!pcl_isfinite (current_frame.rf[0]) ||
270  !pcl_isfinite (current_frame.rf[4]) ||
271  !pcl_isfinite (current_frame.rf[11]))
272  {
273  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
274  getClassName ().c_str (), (*indices_)[idx]);
275  lrf_is_nan = true;
276  }
277 
278  // Allocate enough space to hold the results
279  // \note This resize is irrelevant for a radiusSearch ().
280  std::vector<int> nn_indices (k_);
281  std::vector<float> nn_dists (k_);
282 
283  if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
284  nn_dists) == 0)
285  {
286  // Copy into the resultant cloud
287  for (int d = 0; d < shot[tid].size (); ++d)
288  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
289  for (int d = 0; d < 9; ++d)
290  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
291 
292  output.is_dense = false;
293  continue;
294  }
295 
296  // Estimate the SHOT at each patch
297  this->computePointSHOT (idx, nn_indices, nn_dists, shot[tid]);
298 
299  // Copy into the resultant cloud
300  for (int d = 0; d < shot[tid].size (); ++d)
301  output.points[idx].descriptor[d] = shot[tid][d];
302  for (int d = 0; d < 9; ++d)
303  output.points[idx].rf[d] = frames_->points[idx].rf[(4 * (d / 3) + (d % 3))];
304  }
305  delete[] shot;
306 }
307 
309 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
310 pcl::SHOTColorEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
311 {
312  if (threads_ < 0)
313  threads_ = 1;
314 
315  descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
316  descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
317 
318  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
319  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
320  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
321  );
322 
323  sqradius_ = search_radius_ * search_radius_;
324  radius3_4_ = (search_radius_ * 3) / 4;
325  radius1_4_ = search_radius_ / 4;
326  radius1_2_ = search_radius_ / 2;
327 
328  int data_size = static_cast<int> (indices_->size ());
329  Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
330 
331  for (int i = 0; i < threads_; i++)
332  shot[i].setZero (descLength_);
333 
334  output.is_dense = true;
335  // Iterating over the entire index vector
336 #pragma omp parallel for num_threads(threads_)
337  for (int idx = 0; idx < data_size; ++idx)
338  {
339 #ifdef _OPENMP
340  int tid = omp_get_thread_num ();
341 #else
342  int tid = 0;
343 #endif
344  // Allocate enough space to hold the results
345  // \note This resize is irrelevant for a radiusSearch ().
346  std::vector<int> nn_indices (k_);
347  std::vector<float> nn_dists (k_);
348 
349  bool lrf_is_nan = false;
350  const PointRFT& current_frame = (*frames_)[idx];
351  if (!pcl_isfinite (current_frame.rf[0]) ||
352  !pcl_isfinite (current_frame.rf[4]) ||
353  !pcl_isfinite (current_frame.rf[11]))
354  {
355  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
356  getClassName ().c_str (), (*indices_)[idx]);
357  lrf_is_nan = true;
358  }
359 
360  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
361  lrf_is_nan ||
362  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
363  {
364  // Copy into the resultant cloud
365  for (int d = 0; d < shot[tid].size (); ++d)
366  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
367  for (int d = 0; d < 9; ++d)
368  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
369 
370  output.is_dense = false;
371  continue;
372  }
373 
374  // Estimate the SHOT at each patch
375  this->computePointSHOT (idx, nn_indices, nn_dists, shot[tid]);
376 
377  // Copy into the resultant cloud
378  for (int d = 0; d < shot[tid].size (); ++d)
379  output.points[idx].descriptor[d] = shot[tid][d];
380  for (int d = 0; d < 9; ++d)
381  output.points[idx].rf[d] = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
382  }
383 
384  delete[] shot;
385 }
386 
388 template<typename PointNT, typename PointRFT> void
389 pcl::SHOTEstimationOMP<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::computeFeature (PointCloudOut &output)
390 {
391  if (threads_ < 0)
392  threads_ = 1;
393 
394  descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
395  descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
396 
397  sqradius_ = search_radius_ * search_radius_;
398  radius3_4_ = (search_radius_ * 3) / 4;
399  radius1_4_ = search_radius_ / 4;
400  radius1_2_ = search_radius_ / 2;
401 
402  //if (output.points[0].descriptor.size () != static_cast<size_t> (descLength_))
403  for (size_t idx = 0; idx < indices_->size (); ++idx)
404  output.points[idx].descriptor.resize (descLength_);
405 
406  int data_size = static_cast<int> (indices_->size ());
407  Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
408 
409  for (int i = 0; i < threads_; i++)
410  shot[i].setZero (descLength_);
411 
412  output.is_dense = true;
413  // Iterating over the entire index vector
414 #pragma omp parallel for num_threads(threads_)
415  for (int idx = 0; idx < data_size; ++idx)
416  {
417 #ifdef _OPENMP
418  int tid = omp_get_thread_num ();
419 #else
420  int tid = 0;
421 #endif
422  // Allocate enough space to hold the results
423  // \note This resize is irrelevant for a radiusSearch ().
424  std::vector<int> nn_indices (k_);
425  std::vector<float> nn_dists (k_);
426 
427  bool lrf_is_nan = false;
428  const PointRFT& current_frame = (*frames_)[idx];
429  if (!pcl_isfinite (current_frame.rf[0]) ||
430  !pcl_isfinite (current_frame.rf[4]) ||
431  !pcl_isfinite (current_frame.rf[11]))
432  {
433  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
434  getClassName ().c_str (), (*indices_)[idx]);
435  lrf_is_nan = true;
436  }
437 
438  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
439  lrf_is_nan ||
440  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
441  {
442  // Copy into the resultant cloud
443  for (int d = 0; d < shot[tid].size (); ++d)
444  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
445  for (int d = 0; d < 9; ++d)
446  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
447 
448  output.is_dense = false;
449  continue;
450  }
451 
452  // Estimate the SHOT at each patch
453  this->computePointSHOT (idx, nn_indices, nn_dists, shot[tid]);
454 
455  // Copy into the resultant cloud
456  for (int d = 0; d < shot[tid].size (); ++d)
457  output.points[idx].descriptor[d] = shot[tid][d];
458  for (int d = 0; d < 9; ++d)
459  output.points[idx].rf[d] = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
460  }
461 
462  delete[] shot;
463 }
464 
465 #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT,RFT>;
466 #define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP<T,NT,OutT,RFT>;
467 
468 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_
Generated on Fri Mar 8 2013 12:50:46 for Point Cloud Library (PCL) by   doxygen 1.8.3.1