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
69 output.width = output.height = 0;
70 output.points.clear ();
74 output.header = input_->header;
80 output.width = output.height = 1;
81 output.is_dense = input_->is_dense;
82 output.points.resize (1);
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 for (
int i = 0; i < 4; ++i)
102 hist_f_[i].setZero (nr_bins_f_[i]);
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 for (
int i = 0; i < 3; ++i)
144 const int raw_index =
static_cast<int> (std::floor (nr_bins_f_[i] * ((pfh_tuple[i] + M_PI) * d_pi_)));
145 const int h_index = std::max(std::min(raw_index, nr_bins_f_[i] - 1), 0);
146 hist_f_[i] (h_index) += hist_incr;
149 if (hist_incr_size_component)
152 if (normalize_distances_)
153 h_index =
static_cast<int> (std::floor (nr_bins_f_[3] * (pfh_tuple[3] / distance_normalization_factor)));
155 h_index =
static_cast<int> (
pcl_round (pfh_tuple[3] * 100));
157 h_index = std::max (std::min (h_index, nr_bins_f_[3] - 1), 0);
158 hist_f_[3] (h_index) += hist_incr_size_component;
163 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
167 Eigen::Vector4f xyz_centroid (0, 0, 0, 0);
169 if (use_given_centroid_)
170 xyz_centroid = centroid_to_use_;
175 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
178 if (use_given_normal_)
179 normal_centroid = normal_to_use_;
183 if (normals_->is_dense)
185 for (
const auto& index: *indices_)
187 normal_centroid.noalias () += normals_->points[index].getNormalVector4fMap ();
189 cp = indices_->size();
194 for (
const auto& index: *indices_)
196 if (!std::isfinite (normals_->points[index].normal[0]) ||
197 !std::isfinite (normals_->points[index].normal[1]) ||
198 !std::isfinite (normals_->points[index].normal[2]))
200 normal_centroid.noalias () += normals_->points[index].getNormalVector4fMap ();
204 normal_centroid /=
static_cast<float> (
cp);
208 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
209 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
213 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
216 hist_vp_.setZero (nr_bins_vp_);
218 float hist_incr = 1.0;
220 hist_incr = 100.0 /
static_cast<double> (indices_->size ());
222 for (
const auto& index: *indices_)
224 Eigen::Vector4f normal (normals_->points[index].normal[0],
225 normals_->points[index].normal[1],
226 normals_->points[index].normal[2], 0);
228 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
229 std::size_t fi =
static_cast<std::size_t
> (std::floor (alpha * hist_vp_.size ()));
230 fi = std::max<std::size_t> (0u, fi);
231 fi = std::min<std::size_t> (hist_vp_.size () - 1, fi);
233 hist_vp_ [fi] += hist_incr;
237 output.points.resize (1);
242 auto outPtr = std::begin (output.points[0].histogram);
244 for (
int i = 0; i < 4; ++i)
246 outPtr = std::copy_n (hist_f_[i].data (), hist_f_[i].size (), outPtr);
248 outPtr = std::copy_n (hist_vp_.data (), hist_vp_.size (), outPtr);
251 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
253 #endif // PCL_FEATURES_IMPL_VFH_H_