42 #include <pcl/features/shot_omp.h>
44 #include <pcl/common/point_tests.h>
46 #include <pcl/features/shot_lrf_omp.h>
49 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
54 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
59 if (this->getKSearch () != 0)
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 ());
69 lrf_estimator->
setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
79 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
87 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
92 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
97 if (this->getKSearch () != 0)
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 ());
107 lrf_estimator->
setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
117 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
125 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
130 threads_ = omp_get_num_procs();
135 threads_ = nr_threads;
139 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
142 descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
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;
149 assert(descLength_ == 352);
151 output.is_dense =
true;
153 #pragma omp parallel for \
156 num_threads(threads_)
157 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
160 Eigen::VectorXf shot;
161 shot.setZero (descLength_);
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]))
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]);
177 std::vector<float> nn_dists (k_);
179 if (!
isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
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 ();
188 output.is_dense =
false;
193 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
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)
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];
208 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
213 threads_ = omp_get_num_procs();
218 threads_ = nr_threads;
222 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
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;
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)
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;
238 output.is_dense =
true;
240 #pragma omp parallel for \
243 num_threads(threads_)
244 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
246 Eigen::VectorXf shot;
247 shot.setZero (descLength_);
252 std::vector<float> nn_dists (k_);
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]))
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 (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 ();
275 output.is_dense =
false;
280 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
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)
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];
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>;