Point Cloud Library (PCL)  1.7.1
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
66  typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>());
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
104  typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>());
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  descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
127 
128  sqradius_ = search_radius_ * search_radius_;
129  radius3_4_ = (search_radius_ * 3) / 4;
130  radius1_4_ = search_radius_ / 4;
131  radius1_2_ = search_radius_ / 2;
132 
133  assert(descLength_ == 352);
134 
135  int data_size = static_cast<int> (indices_->size ());
136 
137  output.is_dense = true;
138  // Iterating over the entire index vector
139 #ifdef _OPENMP
140 #pragma omp parallel for num_threads(threads_)
141 #endif
142  for (int idx = 0; idx < data_size; ++idx)
143  {
144 
145  Eigen::VectorXf shot;
146  shot.setZero (descLength_);
147 
148  bool lrf_is_nan = false;
149  const PointRFT& current_frame = (*frames_)[idx];
150  if (!pcl_isfinite (current_frame.x_axis[0]) ||
151  !pcl_isfinite (current_frame.y_axis[0]) ||
152  !pcl_isfinite (current_frame.z_axis[0]))
153  {
154  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
155  getClassName ().c_str (), (*indices_)[idx]);
156  lrf_is_nan = true;
157  }
158 
159  // Allocate enough space to hold the results
160  // \note This resize is irrelevant for a radiusSearch ().
161  std::vector<int> nn_indices (k_);
162  std::vector<float> nn_dists (k_);
163 
164  if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
165  nn_dists) == 0)
166  {
167  // Copy into the resultant cloud
168  for (int d = 0; d < shot.size (); ++d)
169  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
170  for (int d = 0; d < 9; ++d)
171  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
172 
173  output.is_dense = false;
174  continue;
175  }
176 
177  // Estimate the SHOT at each patch
178  this->computePointSHOT (idx, nn_indices, nn_dists, shot);
179 
180  // Copy into the resultant cloud
181  for (int d = 0; d < shot.size (); ++d)
182  output.points[idx].descriptor[d] = shot[d];
183  for (int d = 0; d < 3; ++d)
184  {
185  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
186  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
187  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
188  }
189  }
190 }
191 
192 //////////////////////////////////////////////////////////////////////////////////////////////
193 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
195 {
196  descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
197  descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
198 
199  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
200  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
201  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
202  );
203 
204  sqradius_ = search_radius_ * search_radius_;
205  radius3_4_ = (search_radius_ * 3) / 4;
206  radius1_4_ = search_radius_ / 4;
207  radius1_2_ = search_radius_ / 2;
208 
209  int data_size = static_cast<int> (indices_->size ());
210 
211  output.is_dense = true;
212  // Iterating over the entire index vector
213 #ifdef _OPENMP
214 #pragma omp parallel for num_threads(threads_)
215 #endif
216  for (int idx = 0; idx < data_size; ++idx)
217  {
218  Eigen::VectorXf shot;
219  shot.setZero (descLength_);
220 
221  // Allocate enough space to hold the results
222  // \note This resize is irrelevant for a radiusSearch ().
223  std::vector<int> nn_indices (k_);
224  std::vector<float> nn_dists (k_);
225 
226  bool lrf_is_nan = false;
227  const PointRFT& current_frame = (*frames_)[idx];
228  if (!pcl_isfinite (current_frame.x_axis[0]) ||
229  !pcl_isfinite (current_frame.y_axis[0]) ||
230  !pcl_isfinite (current_frame.z_axis[0]))
231  {
232  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
233  getClassName ().c_str (), (*indices_)[idx]);
234  lrf_is_nan = true;
235  }
236 
237  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
238  lrf_is_nan ||
239  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
240  {
241  // Copy into the resultant cloud
242  for (int d = 0; d < shot.size (); ++d)
243  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
244  for (int d = 0; d < 9; ++d)
245  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
246 
247  output.is_dense = false;
248  continue;
249  }
250 
251  // Estimate the SHOT at each patch
252  this->computePointSHOT (idx, nn_indices, nn_dists, shot);
253 
254  // Copy into the resultant cloud
255  for (int d = 0; d < shot.size (); ++d)
256  output.points[idx].descriptor[d] = shot[d];
257  for (int d = 0; d < 3; ++d)
258  {
259  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
260  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
261  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
262  }
263  }
264 }
265 
266 #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT,RFT>;
267 #define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP<T,NT,OutT,RFT>;
268 
269 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_