41 #ifndef PCL_FEATURES_IMPL_VFH_H_
42 #define PCL_FEATURES_IMPL_VFH_H_
44 #include <pcl/features/vfh.h>
45 #include <pcl/features/pfh_tools.h>
50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
53 if (input_->points.size () < 2 || (surface_ && surface_->points.size () < 2))
55 PCL_ERROR (
"[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n");
58 if (search_radius_ == 0 && k_ == 0)
64 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
74 output.
header = input_->header;
85 computeFeature (output);
91 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
93 const Eigen::Vector4f ¢roid_n,
96 const std::vector<int> &indices)
98 Eigen::Vector4f pfh_tuple;
100 hist_f1_.setZero (nr_bins_f1_);
101 hist_f2_.setZero (nr_bins_f2_);
102 hist_f3_.setZero (nr_bins_f3_);
103 hist_f4_.setZero (nr_bins_f4_);
114 double distance_normalization_factor = 1.0;
115 if (normalize_distances_)
117 Eigen::Vector4f max_pt;
120 distance_normalization_factor = (centroid_p - max_pt).norm ();
126 hist_incr = 100.0f /
static_cast<float> (indices.size () - 1);
130 float hist_incr_size_component;
132 hist_incr_size_component = hist_incr;
134 hist_incr_size_component = 0.0;
137 for (
size_t idx = 0; idx < indices.size (); ++idx)
141 normals.
points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
142 pfh_tuple[2], pfh_tuple[3]))
146 int h_index =
static_cast<int> (floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_)));
149 if (h_index >= nr_bins_f1_)
150 h_index = nr_bins_f1_ - 1;
151 hist_f1_ (h_index) += hist_incr;
153 h_index =
static_cast<int> (floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5)));
156 if (h_index >= nr_bins_f2_)
157 h_index = nr_bins_f2_ - 1;
158 hist_f2_ (h_index) += hist_incr;
160 h_index =
static_cast<int> (floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5)));
163 if (h_index >= nr_bins_f3_)
164 h_index = nr_bins_f3_ - 1;
165 hist_f3_ (h_index) += hist_incr;
167 if (normalize_distances_)
168 h_index =
static_cast<int> (floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor)));
170 h_index =
static_cast<int> (pcl_round (pfh_tuple[3] * 100));
174 if (h_index >= nr_bins_f4_)
175 h_index = nr_bins_f4_ - 1;
177 hist_f4_ (h_index) += hist_incr_size_component;
181 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
185 Eigen::Vector4f xyz_centroid (0, 0, 0, 0);
187 if (use_given_centroid_)
188 xyz_centroid = centroid_to_use_;
193 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
197 if (use_given_normal_)
198 normal_centroid = normal_to_use_;
201 if (normals_->is_dense)
203 for (
size_t i = 0; i < indices_->size (); ++i)
205 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
212 for (
size_t i = 0; i < indices_->size (); ++i)
214 if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
216 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
218 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
220 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
224 normal_centroid /=
static_cast<float> (
cp);
228 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
229 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
233 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
236 output.points.resize (1);
241 for (
int d = 0; d < hist_f1_.size (); ++d)
242 output.points[0].histogram[d + 0] = hist_f1_[d];
244 size_t data_size = hist_f1_.size ();
245 for (
int d = 0; d < hist_f2_.size (); ++d)
246 output.points[0].histogram[d + data_size] = hist_f2_[d];
248 data_size += hist_f2_.size ();
249 for (
int d = 0; d < hist_f3_.size (); ++d)
250 output.points[0].histogram[d + data_size] = hist_f3_[d];
252 data_size += hist_f3_.size ();
253 for (
int d = 0; d < hist_f4_.size (); ++d)
254 output.points[0].histogram[d + data_size] = hist_f4_[d];
257 hist_vp_.setZero (nr_bins_vp_);
261 hist_incr = 100.0 /
static_cast<double> (indices_->size ());
265 for (
size_t i = 0; i < indices_->size (); ++i)
267 Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
268 normals_->points[(*indices_)[i]].normal[1],
269 normals_->points[(*indices_)[i]].normal[2], 0);
271 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
272 int fi =
static_cast<int> (floor (alpha *
static_cast<double> (hist_vp_.size ())));
275 if (fi > (
static_cast<int> (hist_vp_.size ()) - 1))
276 fi =
static_cast<int> (hist_vp_.size ()) - 1;
278 hist_vp_ [fi] +=
static_cast<float> (hist_incr);
280 data_size += hist_f4_.size ();
282 for (
int d = 0; d < hist_vp_.size (); ++d)
283 output.points[0].histogram[d + data_size] = hist_vp_[d];
286 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
288 #endif // PCL_FEATURES_IMPL_VFH_H_