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_
void computeFeature(PointCloudOut &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
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).
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
bool initCompute()
This method should get called before starting the actual computation.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
bool initCompute()
This method should get called before starting the actual computation.
PointCloud represents the base class in PCL for storing collections of 3D points. ...