Point Cloud Library (PCL)  1.10.0-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  output.is_dense = true;
150  // Iterating over the entire index vector
151 #ifdef _OPENMP
152 #pragma omp parallel for num_threads(threads_)
153 #endif
154  for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
155  {
156 
157  Eigen::VectorXf shot;
158  shot.setZero (descLength_);
159 
160  bool lrf_is_nan = false;
161  const PointRFT& current_frame = (*frames_)[idx];
162  if (!std::isfinite (current_frame.x_axis[0]) ||
163  !std::isfinite (current_frame.y_axis[0]) ||
164  !std::isfinite (current_frame.z_axis[0]))
165  {
166  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
167  getClassName ().c_str (), (*indices_)[idx]);
168  lrf_is_nan = true;
169  }
170 
171  // Allocate enough space to hold the results
172  // \note This resize is irrelevant for a radiusSearch ().
173  std::vector<int> nn_indices (k_);
174  std::vector<float> nn_dists (k_);
175 
176  if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
177  nn_dists) == 0)
178  {
179  // Copy into the resultant cloud
180  for (Eigen::Index d = 0; d < shot.size (); ++d)
181  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
182  for (int d = 0; d < 9; ++d)
183  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
184 
185  output.is_dense = false;
186  continue;
187  }
188 
189  // Estimate the SHOT at each patch
190  this->computePointSHOT (idx, nn_indices, nn_dists, shot);
191 
192  // Copy into the resultant cloud
193  for (Eigen::Index d = 0; d < shot.size (); ++d)
194  output.points[idx].descriptor[d] = shot[d];
195  for (int d = 0; d < 3; ++d)
196  {
197  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
198  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
199  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
200  }
201  }
202 }
203 
204 //////////////////////////////////////////////////////////////////////////////////////////////
205 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
207 {
208  if (nr_threads == 0)
209 #ifdef _OPENMP
210  threads_ = omp_get_num_procs();
211 #else
212  threads_ = 1;
213 #endif
214  else
215  threads_ = nr_threads;
216 }
217 
218 //////////////////////////////////////////////////////////////////////////////////////////////
219 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
221 {
222  descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
223  descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
224 
225  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
226  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
227  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
228  );
229 
230  sqradius_ = search_radius_ * search_radius_;
231  radius3_4_ = (search_radius_ * 3) / 4;
232  radius1_4_ = search_radius_ / 4;
233  radius1_2_ = search_radius_ / 2;
234 
235  output.is_dense = true;
236  // Iterating over the entire index vector
237 #ifdef _OPENMP
238 #pragma omp parallel for num_threads(threads_)
239 #endif
240  for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
241  {
242  Eigen::VectorXf shot;
243  shot.setZero (descLength_);
244 
245  // Allocate enough space to hold the results
246  // \note This resize is irrelevant for a radiusSearch ().
247  std::vector<int> nn_indices (k_);
248  std::vector<float> nn_dists (k_);
249 
250  bool lrf_is_nan = false;
251  const PointRFT& current_frame = (*frames_)[idx];
252  if (!std::isfinite (current_frame.x_axis[0]) ||
253  !std::isfinite (current_frame.y_axis[0]) ||
254  !std::isfinite (current_frame.z_axis[0]))
255  {
256  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
257  getClassName ().c_str (), (*indices_)[idx]);
258  lrf_is_nan = true;
259  }
260 
261  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
262  lrf_is_nan ||
263  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
264  {
265  // Copy into the resultant cloud
266  for (Eigen::Index d = 0; d < shot.size (); ++d)
267  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
268  for (int d = 0; d < 9; ++d)
269  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
270 
271  output.is_dense = false;
272  continue;
273  }
274 
275  // Estimate the SHOT at each patch
276  this->computePointSHOT (idx, nn_indices, nn_dists, shot);
277 
278  // Copy into the resultant cloud
279  for (Eigen::Index d = 0; d < shot.size (); ++d)
280  output.points[idx].descriptor[d] = shot[d];
281  for (int d = 0; d < 3; ++d)
282  {
283  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
284  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
285  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
286  }
287  }
288 }
289 
290 #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT,RFT>;
291 #define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP<T,NT,OutT,RFT>;
292 
293 #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:55
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
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:220
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
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:206
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:405
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.