40 #ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_
41 #define PCL_FEATURES_IMPL_SHOT_OMP_H_
43 #include <pcl/features/shot_omp.h>
45 #include <pcl/features/shot_lrf_omp.h>
47 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
52 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
57 if (this->getKSearch () != 0)
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 ());
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_);
73 lrf_estimator->setSearchSurface(surface_);
77 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
85 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
90 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
95 if (this->getKSearch () != 0)
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 ());
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_);
111 lrf_estimator->setSearchSurface(surface_);
115 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
123 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
128 threads_ = omp_get_num_procs();
133 threads_ = nr_threads;
137 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
140 descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
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;
147 assert(descLength_ == 352);
149 int data_size =
static_cast<int> (indices_->size ());
154 #pragma omp parallel for num_threads(threads_)
156 for (
int idx = 0; idx < data_size; ++idx)
159 Eigen::VectorXf shot;
160 shot.setZero (descLength_);
162 bool lrf_is_nan =
false;
163 const PointRFT& current_frame = (*frames_)[idx];
164 if (!pcl_isfinite (current_frame.x_axis[0]) ||
165 !pcl_isfinite (current_frame.y_axis[0]) ||
166 !pcl_isfinite (current_frame.z_axis[0]))
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]);
175 std::vector<int> nn_indices (k_);
176 std::vector<float> nn_dists (k_);
178 if (!
isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
182 for (
int 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 ();
192 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
195 for (
int d = 0; d < shot.size (); ++d)
196 output.
points[idx].descriptor[d] = shot[d];
197 for (
int d = 0; d < 3; ++d)
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];
207 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
212 threads_ = omp_get_num_procs();
217 threads_ = nr_threads;
221 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
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;
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)
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;
237 int data_size =
static_cast<int> (indices_->size ());
242 #pragma omp parallel for num_threads(threads_)
244 for (
int idx = 0; idx < data_size; ++idx)
246 Eigen::VectorXf shot;
247 shot.setZero (descLength_);
251 std::vector<int> nn_indices (k_);
252 std::vector<float> nn_dists (k_);
254 bool lrf_is_nan =
false;
255 const PointRFT& current_frame = (*frames_)[idx];
256 if (!pcl_isfinite (current_frame.x_axis[0]) ||
257 !pcl_isfinite (current_frame.y_axis[0]) ||
258 !pcl_isfinite (current_frame.z_axis[0]))
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]);
265 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
267 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
270 for (
int 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 ();
280 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
283 for (
int d = 0; d < shot.size (); ++d)
284 output.
points[idx].descriptor[d] = shot[d];
285 for (
int d = 0; d < 3; ++d)
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];
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>;
297 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_