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>
46 #include <pcl/features/pfh_tools.h>
50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
55 output.width = output.height = 0;
56 output.points.clear ();
63 output.width = output.height = 1;
64 output.points.resize (1);
67 computeFeature (output);
73 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
79 std::vector<pcl::PointIndices> &clusters,
81 unsigned int min_pts_per_cluster,
82 unsigned int max_pts_per_cluster)
86 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->
getInputCloud ()->points.size (), cloud.
points.size ());
91 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
96 std::vector<bool> processed (cloud.
points.size (),
false);
98 std::vector<int> nn_indices;
99 std::vector<float> nn_distances;
101 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
111 seed_queue.push_back (i);
114 for (std::size_t idx = 0; idx != seed_queue.size (); ++idx)
117 if (!tree->
radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances))
123 for (std::size_t j = 1; j < nn_indices.size (); ++j)
125 if (processed[nn_indices[j]])
130 const double dot_p = normals.
points[seed_queue[idx]].getNormalVector3fMap().dot(
131 normals.
points[nn_indices[j]].getNormalVector3fMap());
133 if (std::acos (dot_p) < eps_angle)
135 processed[nn_indices[j]] =
true;
136 seed_queue.emplace_back (nn_indices[j]);
142 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
148 clusters.emplace_back (std::move(r));
154 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
157 std::vector<int> &indices_to_use,
158 std::vector<int> &indices_out,
159 std::vector<int> &indices_in,
162 indices_out.resize (cloud.
points.size ());
163 indices_in.resize (cloud.
points.size ());
168 for (
const int &index : indices_to_use)
170 if (cloud.
points[index].curvature > threshold)
172 indices_out[out] = index;
177 indices_in[in] = index;
182 indices_out.resize (out);
183 indices_in.resize (in);
187 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
193 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
194 output.width = output.height = 0;
195 output.points.clear ();
198 if (normals_->points.size () != surface_->points.size ())
200 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 ());
201 output.width = output.height = 0;
202 output.points.clear ();
206 centroids_dominant_orientations_.clear ();
209 std::vector<int> indices_out;
210 std::vector<int> indices_in;
211 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
214 normals_filtered_cloud->width =
static_cast<std::uint32_t
> (indices_in.size ());
215 normals_filtered_cloud->height = 1;
216 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
218 for (std::size_t i = 0; i < indices_in.size (); ++i)
220 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
221 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
222 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
225 std::vector<pcl::PointIndices> clusters;
227 if(normals_filtered_cloud->points.size() >= min_points_)
231 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
238 n3d.
compute (*normals_filtered_cloud);
241 normals_tree->setInputCloud (normals_filtered_cloud);
243 extractEuclideanClustersSmooth (*normals_filtered_cloud,
244 *normals_filtered_cloud,
248 eps_angle_threshold_,
249 static_cast<unsigned int> (min_points_));
254 vfh.setInputCloud (surface_);
255 vfh.setInputNormals (normals_);
256 vfh.setIndices(indices_);
257 vfh.setSearchMethod (this->tree_);
258 vfh.setUseGivenNormal (
true);
259 vfh.setUseGivenCentroid (
true);
260 vfh.setNormalizeBins (normalize_bins_);
261 vfh.setNormalizeDistance (
true);
262 vfh.setFillSizeComponent (
true);
266 if (!clusters.empty ())
269 for (
const auto &cluster : clusters)
271 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
272 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
274 for (
const auto &index : cluster.indices)
276 avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
277 avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
280 avg_normal /=
static_cast<float> (cluster.indices.size ());
281 avg_centroid /=
static_cast<float> (cluster.indices.size ());
283 Eigen::Vector4f centroid_test;
285 avg_normal.normalize ();
287 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
288 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
291 dominant_normals_.push_back (avg_norm);
292 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
296 output.points.resize (dominant_normals_.size ());
297 output.width =
static_cast<std::uint32_t
> (dominant_normals_.size ());
299 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
302 vfh.setNormalToUse (dominant_normals_[i]);
303 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
305 vfh.compute (vfh_signature);
311 Eigen::Vector4f avg_centroid;
313 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
314 centroids_dominant_orientations_.push_back (cloud_centroid);
317 vfh.setCentroidToUse (cloud_centroid);
318 vfh.setUseGivenNormal (
false);
321 vfh.compute (vfh_signature);
326 output.points[0] = vfh_signature.
points[0];
330 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
332 #endif // PCL_FEATURES_IMPL_VFH_H_