Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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#pragma once
41
42#include <pcl/features/shot_omp.h>
43
44#include <pcl/common/point_tests.h> // for pcl::isFinite
45#include <pcl/common/time.h>
46#include <pcl/features/shot_lrf_omp.h>
47
48
49template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
51{
53 {
54 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
55 return (false);
56 }
57
58 // SHOT cannot work with k-search
59 if (this->getKSearch () != 0)
60 {
61 PCL_ERROR(
62 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
63 getClassName().c_str ());
64 return (false);
65 }
66
67 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
69 lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
70 lrf_estimator->setInputCloud (input_);
71 lrf_estimator->setIndices (indices_);
72 lrf_estimator->setNumberOfThreads(threads_);
73
74 if (!fake_surface_)
75 lrf_estimator->setSearchSurface(surface_);
76
78 {
79 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
80 return (false);
81 }
82
83 return (true);
84}
85
86//////////////////////////////////////////////////////////////////////////////////////////////
87template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
89{
91 {
92 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
93 return (false);
94 }
95
96 // SHOT cannot work with k-search
97 if (this->getKSearch () != 0)
98 {
99 PCL_ERROR(
100 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
101 getClassName().c_str ());
102 return (false);
103 }
104
105 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP
107 lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
108 lrf_estimator->setInputCloud (input_);
109 lrf_estimator->setIndices (indices_);
110 lrf_estimator->setNumberOfThreads(threads_);
111
112 if (!fake_surface_)
113 lrf_estimator->setSearchSurface(surface_);
114
116 {
117 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
118 return (false);
119 }
120
121 return (true);
122}
123
124//////////////////////////////////////////////////////////////////////////////////////////////
125template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
127{
128 if (nr_threads == 0)
129#ifdef _OPENMP
130 threads_ = omp_get_num_procs();
131#else
132 threads_ = 1;
133#endif
134 else
135 threads_ = nr_threads;
136}
137
138//////////////////////////////////////////////////////////////////////////////////////////////
139template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
141{
142 descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
143
144 sqradius_ = search_radius_ * search_radius_;
145 radius3_4_ = (search_radius_ * 3) / 4;
146 radius1_4_ = search_radius_ / 4;
147 radius1_2_ = search_radius_ / 2;
148
149 assert(descLength_ == 352);
150
151 output.is_dense = true;
152 // Iterating over the entire index vector
153#pragma omp parallel for \
154 default(none) \
155 shared(output) \
156 num_threads(threads_)
157 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
158 {
159
160 Eigen::VectorXf shot;
161 shot.setZero (descLength_);
162
163 bool lrf_is_nan = false;
164 const PointRFT& current_frame = (*frames_)[idx];
165 if (!std::isfinite (current_frame.x_axis[0]) ||
166 !std::isfinite (current_frame.y_axis[0]) ||
167 !std::isfinite (current_frame.z_axis[0]))
168 {
169 PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
170 getClassName ().c_str (), (*indices_)[idx]);
171 lrf_is_nan = true;
172 }
173
174 // Allocate enough space to hold the results
175 // \note This resize is irrelevant for a radiusSearch ().
176 pcl::Indices nn_indices (k_);
177 std::vector<float> nn_dists (k_);
178
179 if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
180 nn_dists) == 0)
181 {
182 // Copy into the resultant cloud
183 for (Eigen::Index d = 0; d < shot.size (); ++d)
184 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
185 for (int d = 0; d < 9; ++d)
186 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
187
188 output.is_dense = false;
189 continue;
190 }
191
192 // Estimate the SHOT at each patch
193 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
194
195 // Copy into the resultant cloud
196 for (Eigen::Index d = 0; d < shot.size (); ++d)
197 output[idx].descriptor[d] = shot[d];
198 for (int d = 0; d < 3; ++d)
199 {
200 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
201 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
202 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
203 }
204 }
205}
206
207//////////////////////////////////////////////////////////////////////////////////////////////
208template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
210{
211 if (nr_threads == 0)
212#ifdef _OPENMP
213 threads_ = omp_get_num_procs();
214#else
215 threads_ = 1;
216#endif
217 else
218 threads_ = nr_threads;
219}
220
221//////////////////////////////////////////////////////////////////////////////////////////////
222template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
224{
225 descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
226 descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
227
228 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
229 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
230 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
231 );
232
233 sqradius_ = search_radius_ * search_radius_;
234 radius3_4_ = (search_radius_ * 3) / 4;
235 radius1_4_ = search_radius_ / 4;
236 radius1_2_ = search_radius_ / 2;
237
238 output.is_dense = true;
239 // Iterating over the entire index vector
240#pragma omp parallel for \
241 default(none) \
242 shared(output) \
243 num_threads(threads_)
244 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->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 pcl::Indices 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[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
272 for (int d = 0; d < 9; ++d)
273 output[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[idx].descriptor[d] = shot[d];
285 for (int d = 0; d < 3; ++d)
286 {
287 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
288 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
289 output[idx].rf[d + 6] = (*frames_)[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
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:146
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:198
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition feature.h:440
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
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:209
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition shot_omp.h:176
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:223
bool initCompute() override
This method should get called before starting the actual computation.
Definition shot_omp.hpp:88
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition shot_omp.hpp:126
bool initCompute() override
This method should get called before starting the actual computation.
Definition shot_omp.hpp:50
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:140
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition shot_omp.h:94
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
shared_ptr< SHOTLocalReferenceFrameEstimationOMP< PointInT, PointOutT > > Ptr
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Define methods for measuring time spent in code blocks.
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133