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);
128 float hist_incr_size_component = 0;;
130 hist_incr_size_component = hist_incr;
133 for (
const int &index : indices)
137 normals.
points[index].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
138 pfh_tuple[2], pfh_tuple[3]))
142 int h_index = static_cast<int> (std::floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_)));
145 if (h_index >= nr_bins_f1_)
146 h_index = nr_bins_f1_ - 1;
147 hist_f1_ (h_index) += hist_incr;
149 h_index = static_cast<int> (std::floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5)));
152 if (h_index >= nr_bins_f2_)
153 h_index = nr_bins_f2_ - 1;
154 hist_f2_ (h_index) += hist_incr;
156 h_index = static_cast<int> (std::floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5)));
159 if (h_index >= nr_bins_f3_)
160 h_index = nr_bins_f3_ - 1;
161 hist_f3_ (h_index) += hist_incr;
163 if (normalize_distances_)
164 h_index = static_cast<int> (std::floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor)));
166 h_index = static_cast<int> (
pcl_round (pfh_tuple[3] * 100));
170 if (h_index >= nr_bins_f4_)
171 h_index = nr_bins_f4_ - 1;
173 hist_f4_ (h_index) += hist_incr_size_component;
177 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
181 Eigen::Vector4f xyz_centroid (0, 0, 0, 0);
183 if (use_given_centroid_)
184 xyz_centroid = centroid_to_use_;
189 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
193 if (use_given_normal_)
194 normal_centroid = normal_to_use_;
197 if (normals_->is_dense)
199 for (
const auto& index: *indices_)
201 normal_centroid.noalias () += normals_->points[index].getNormalVector4fMap ();
203 cp = indices_->size();
208 for (
const auto& index: *indices_)
210 if (!std::isfinite (normals_->points[index].normal[0]) ||
211 !std::isfinite (normals_->points[index].normal[1]) ||
212 !std::isfinite (normals_->points[index].normal[2]))
214 normal_centroid.noalias () += normals_->points[index].getNormalVector4fMap ();
218 normal_centroid /= static_cast<float> (cp);
222 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
223 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
227 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
230 hist_vp_.setZero (nr_bins_vp_);
232 float hist_incr = 1.0;
234 hist_incr = 100.0 / static_cast<double> (indices_->size ());
236 for (
const auto& index: *indices_)
238 Eigen::Vector4f normal (normals_->points[index].normal[0],
239 normals_->points[index].normal[1],
240 normals_->points[index].normal[2], 0);
242 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
243 int fi = static_cast<int> (std::floor (alpha * static_cast<double> (hist_vp_.size ())));
246 if (fi > (static_cast<int> (hist_vp_.size ()) - 1))
247 fi = static_cast<int> (hist_vp_.size ()) - 1;
249 hist_vp_ [fi] += hist_incr;
253 output.points.resize (1);
258 auto outPtr = std::begin (output.points[0].histogram);
260 outPtr = std::copy_n (hist_f1_.data (), hist_f1_.size (), outPtr);
261 outPtr = std::copy_n (hist_f2_.data (), hist_f2_.size (), outPtr);
262 outPtr = std::copy_n (hist_f3_.data (), hist_f3_.size (), outPtr);
263 outPtr = std::copy_n (hist_f4_.data (), hist_f4_.size (), outPtr);
264 outPtr = std::copy_n (hist_vp_.data (), hist_vp_.size (), outPtr);
267 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
269 #endif // PCL_FEATURES_IMPL_VFH_H_