Point Cloud Library (PCL)  1.9.1-dev
shot_omp.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  *
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_
41 #define PCL_FEATURES_IMPL_SHOT_OMP_H_
42 
43 #include <pcl/features/shot_omp.h>
44 #include <pcl/common/time.h>
45 #include <pcl/features/shot_lrf_omp.h>
46 
47 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
49 {
51  {
52  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
53  return (false);
54  }
55 
56  // SHOT cannot work with k-search
57  if (this->getKSearch () != 0)
58  {
59  PCL_ERROR(
60  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
61  getClassName().c_str ());
62  return (false);
63  }
64 
65  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
67  lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
68  lrf_estimator->setInputCloud (input_);
69  lrf_estimator->setIndices (indices_);
70  lrf_estimator->setNumberOfThreads(threads_);
71 
72  if (!fake_surface_)
73  lrf_estimator->setSearchSurface(surface_);
74 
76  {
77  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
78  return (false);
79  }
80 
81  return (true);
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////
85 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
87 {
89  {
90  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
91  return (false);
92  }
93 
94  // SHOT cannot work with k-search
95  if (this->getKSearch () != 0)
96  {
97  PCL_ERROR(
98  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
99  getClassName().c_str ());
100  return (false);
101  }
102 
103  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
105  lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
106  lrf_estimator->setInputCloud (input_);
107  lrf_estimator->setIndices (indices_);
108  lrf_estimator->setNumberOfThreads(threads_);
109 
110  if (!fake_surface_)
111  lrf_estimator->setSearchSurface(surface_);
112 
114  {
115  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
116  return (false);
117  }
118 
119  return (true);
120 }
121 
122 //////////////////////////////////////////////////////////////////////////////////////////////
123 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
125 {
126  if (nr_threads == 0)
127 #ifdef _OPENMP
128  threads_ = omp_get_num_procs();
129 #else
130  threads_ = 1;
131 #endif
132  else
133  threads_ = nr_threads;
134 }
135 
136 //////////////////////////////////////////////////////////////////////////////////////////////
137 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
139 {
140  descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
141 
142  sqradius_ = search_radius_ * search_radius_;
143  radius3_4_ = (search_radius_ * 3) / 4;
144  radius1_4_ = search_radius_ / 4;
145  radius1_2_ = search_radius_ / 2;
146 
147  assert(descLength_ == 352);
148 
149  int data_size = static_cast<int> (indices_->size ());
150 
151  output.is_dense = true;
152  // Iterating over the entire index vector
153 #ifdef _OPENMP
154 #pragma omp parallel for num_threads(threads_)
155 #endif
156  for (int idx = 0; idx < data_size; ++idx)
157  {
158 
159  Eigen::VectorXf shot;
160  shot.setZero (descLength_);
161 
162  bool lrf_is_nan = false;
163  const PointRFT& current_frame = (*frames_)[idx];
164  if (!std::isfinite (current_frame.x_axis[0]) ||
165  !std::isfinite (current_frame.y_axis[0]) ||
166  !std::isfinite (current_frame.z_axis[0]))
167  {
168  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
169  getClassName ().c_str (), (*indices_)[idx]);
170  lrf_is_nan = true;
171  }
172 
173  // Allocate enough space to hold the results
174  // \note This resize is irrelevant for a radiusSearch ().
175  std::vector<int> nn_indices (k_);
176  std::vector<float> nn_dists (k_);
177 
178  if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
179  nn_dists) == 0)
180  {
181  // Copy into the resultant cloud
182  for (Eigen::Index d = 0; d < shot.size (); ++d)
183  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
184  for (int d = 0; d < 9; ++d)
185  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
186 
187  output.is_dense = false;
188  continue;
189  }
190 
191  // Estimate the SHOT at each patch
192  this->computePointSHOT (idx, nn_indices, nn_dists, shot);
193 
194  // Copy into the resultant cloud
195  for (Eigen::Index d = 0; d < shot.size (); ++d)
196  output.points[idx].descriptor[d] = shot[d];
197  for (int d = 0; d < 3; ++d)
198  {
199  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
200  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
201  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
202  }
203  }
204 }
205 
206 //////////////////////////////////////////////////////////////////////////////////////////////
207 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
209 {
210  if (nr_threads == 0)
211 #ifdef _OPENMP
212  threads_ = omp_get_num_procs();
213 #else
214  threads_ = 1;
215 #endif
216  else
217  threads_ = nr_threads;
218 }
219 
220 //////////////////////////////////////////////////////////////////////////////////////////////
221 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
223 {
224  descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
225  descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
226 
227  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
228  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
229  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
230  );
231 
232  sqradius_ = search_radius_ * search_radius_;
233  radius3_4_ = (search_radius_ * 3) / 4;
234  radius1_4_ = search_radius_ / 4;
235  radius1_2_ = search_radius_ / 2;
236 
237  int data_size = static_cast<int> (indices_->size ());
238 
239  output.is_dense = true;
240  // Iterating over the entire index vector
241 #ifdef _OPENMP
242 #pragma omp parallel for num_threads(threads_)
243 #endif
244  for (int idx = 0; idx < data_size; ++idx)
245  {
246  Eigen::VectorXf shot;
247  shot.setZero (descLength_);
248 
249  // Allocate enough space to hold the results
250  // \note This resize is irrelevant for a radiusSearch ().
251  std::vector<int> nn_indices (k_);
252  std::vector<float> nn_dists (k_);
253 
254  bool lrf_is_nan = false;
255  const PointRFT& current_frame = (*frames_)[idx];
256  if (!std::isfinite (current_frame.x_axis[0]) ||
257  !std::isfinite (current_frame.y_axis[0]) ||
258  !std::isfinite (current_frame.z_axis[0]))
259  {
260  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
261  getClassName ().c_str (), (*indices_)[idx]);
262  lrf_is_nan = true;
263  }
264 
265  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
266  lrf_is_nan ||
267  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
268  {
269  // Copy into the resultant cloud
270  for (Eigen::Index d = 0; d < shot.size (); ++d)
271  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
272  for (int d = 0; d < 9; ++d)
273  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
274 
275  output.is_dense = false;
276  continue;
277  }
278 
279  // Estimate the SHOT at each patch
280  this->computePointSHOT (idx, nn_indices, nn_dists, shot);
281 
282  // Copy into the resultant cloud
283  for (Eigen::Index d = 0; d < shot.size (); ++d)
284  output.points[idx].descriptor[d] = shot[d];
285  for (int d = 0; d < 3; ++d)
286  {
287  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
288  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
289  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
290  }
291  }
292 }
293 
294 #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT,RFT>;
295 #define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP<T,NT,OutT,RFT>;
296 
297 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
Definition: feature.h:148
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
void computeFeature(PointCloudOut &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition: shot_omp.hpp:222
bool initCompute() override
This method should get called before starting the actual computation.
Definition: shot_omp.hpp:86
bool initCompute() override
This method should get called before starting the actual computation.
Definition: shot_omp.hpp:48
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
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition: shot_omp.hpp:124
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition: shot_lrf_omp.h:66
boost::shared_ptr< SHOTLocalReferenceFrameEstimationOMP< PointInT, PointOutT > > Ptr
Definition: shot_lrf_omp.h:69
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition: shot_omp.hpp:208
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
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:431
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:447
void computeFeature(PointCloudOut &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition: shot_omp.hpp:138
Define methods for measuring time spent in code blocks.