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_));
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_));
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);
152 #pragma omp parallel for num_threads(threads_)
154 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
157 Eigen::VectorXf shot;
158 shot.setZero (descLength_);
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]))
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]);
173 std::vector<int> nn_indices (k_);
174 std::vector<float> nn_dists (k_);
176 if (!
isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
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 ();
190 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
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)
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];
205 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
210 threads_ = omp_get_num_procs();
215 threads_ = nr_threads;
219 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
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;
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)
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;
238 #pragma omp parallel for num_threads(threads_)
240 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
242 Eigen::VectorXf shot;
243 shot.setZero (descLength_);
247 std::vector<int> nn_indices (k_);
248 std::vector<float> nn_dists (k_);
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]))
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]);
261 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
263 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
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 ();
276 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
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)
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];
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>;
293 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_