41 #ifndef PCL_FEATURES_IMPL_CVFH_H_
42 #define PCL_FEATURES_IMPL_CVFH_H_
44 #include <pcl/features/cvfh.h>
45 #include <pcl/features/normal_3d.h>
49 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
54 output.width = output.height = 0;
62 output.width = output.height = 1;
66 computeFeature (output);
72 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
78 std::vector<pcl::PointIndices> &clusters,
80 unsigned int min_pts_per_cluster,
81 unsigned int max_pts_per_cluster)
85 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
86 "dataset (%zu) than the input cloud (%zu)!\n",
88 static_cast<std::size_t
>(cloud.
size()));
91 if (cloud.
size () != normals.
size ())
93 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
94 "cloud (%zu) different than normals (%zu)!\n",
95 static_cast<std::size_t
>(cloud.
size()),
96 static_cast<std::size_t
>(normals.
size()));
101 std::vector<bool> processed (cloud.
size (),
false);
104 std::vector<float> nn_distances;
106 for (std::size_t i = 0; i < cloud.
size (); ++i)
116 seed_queue.push_back (i);
119 for (std::size_t idx = 0; idx != seed_queue.size (); ++idx)
122 if (!tree->
radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances))
128 for (std::size_t j = 1; j < nn_indices.size (); ++j)
130 if (processed[nn_indices[j]])
135 const double dot_p = normals[seed_queue[idx]].getNormalVector3fMap().dot(
136 normals[nn_indices[j]].getNormalVector3fMap());
138 if (std::acos (dot_p) < eps_angle)
140 processed[nn_indices[j]] =
true;
147 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
153 clusters.emplace_back (std::move(r));
159 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
167 indices_out.resize (cloud.
size ());
168 indices_in.resize (cloud.
size ());
173 for (
const auto &index : indices_to_use)
175 if (cloud[index].curvature > threshold)
177 indices_out[out] = index;
182 indices_in[in] = index;
187 indices_out.resize (out);
188 indices_in.resize (in);
192 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
198 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
199 output.width = output.height = 0;
203 if (normals_->size () != surface_->size ())
205 PCL_ERROR (
"[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
206 output.width = output.height = 0;
211 centroids_dominant_orientations_.clear ();
216 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
219 normals_filtered_cloud->width = indices_in.size ();
220 normals_filtered_cloud->height = 1;
221 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
223 for (std::size_t i = 0; i < indices_in.size (); ++i)
225 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
226 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
227 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
230 std::vector<pcl::PointIndices> clusters;
232 if(normals_filtered_cloud->size() >= min_points_)
236 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
243 n3d.
compute (*normals_filtered_cloud);
246 normals_tree->setInputCloud (normals_filtered_cloud);
248 extractEuclideanClustersSmooth (*normals_filtered_cloud,
249 *normals_filtered_cloud,
253 eps_angle_threshold_,
254 static_cast<unsigned int> (min_points_));
259 vfh.setInputCloud (surface_);
260 vfh.setInputNormals (normals_);
261 vfh.setIndices(indices_);
262 vfh.setSearchMethod (this->tree_);
263 vfh.setUseGivenNormal (
true);
264 vfh.setUseGivenCentroid (
true);
265 vfh.setNormalizeBins (normalize_bins_);
266 vfh.setNormalizeDistance (
true);
267 vfh.setFillSizeComponent (
true);
271 if (!clusters.empty ())
274 for (
const auto &cluster : clusters)
276 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
277 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
279 for (
const auto &index : cluster.indices)
281 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
282 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
285 avg_normal /=
static_cast<float> (cluster.indices.size ());
286 avg_centroid /=
static_cast<float> (cluster.indices.size ());
288 Eigen::Vector4f centroid_test;
290 avg_normal.normalize ();
292 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
293 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
296 dominant_normals_.push_back (avg_norm);
297 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
301 output.resize (dominant_normals_.size ());
302 output.width = dominant_normals_.size ();
304 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
307 vfh.setNormalToUse (dominant_normals_[i]);
308 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
310 vfh.compute (vfh_signature);
311 output[i] = vfh_signature[0];
316 Eigen::Vector4f avg_centroid;
318 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
319 centroids_dominant_orientations_.push_back (cloud_centroid);
322 vfh.setCentroidToUse (cloud_centroid);
323 vfh.setUseGivenNormal (
false);
326 vfh.compute (vfh_signature);
331 output[0] = vfh_signature[0];
335 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
337 #endif // PCL_FEATURES_IMPL_VFH_H_