40 #ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_ 41 #define PCL_FEATURES_IMPL_SHOT_OMP_H_ 43 #include <pcl/features/shot_omp.h> 44 #include <pcl/common/time.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 126 descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
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;
133 assert(descLength_ == 352);
135 int data_size =
static_cast<int> (indices_->size ());
140 #pragma omp parallel for num_threads(threads_) 142 for (
int idx = 0; idx < data_size; ++idx)
145 Eigen::VectorXf shot;
146 shot.setZero (descLength_);
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]))
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]);
161 std::vector<int> nn_indices (k_);
162 std::vector<float> nn_dists (k_);
164 if (!
isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices,
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 ();
178 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
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)
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];
193 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void 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;
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)
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;
209 int data_size =
static_cast<int> (indices_->size ());
214 #pragma omp parallel for num_threads(threads_) 216 for (
int idx = 0; idx < data_size; ++idx)
218 Eigen::VectorXf shot;
219 shot.setZero (descLength_);
223 std::vector<int> nn_indices (k_);
224 std::vector<float> nn_dists (k_);
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]))
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]);
237 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
239 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
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 ();
252 this->computePointSHOT (idx, nn_indices, nn_dists, shot);
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)
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];
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>; 269 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_ bool initCompute()
This method should get called before starting the actual computation.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void computeFeature(PointCloudOut &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void computeFeature(PointCloudOut &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool initCompute()
This method should get called before starting the actual computation.
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...